Optimization of fast robot motion with distance field

Abstract

To provide a technique for optimizing a robot collision avoidance motion by using a distance field constraint function.SOLUTION: CAD or sensor data indicating obstacles in a robot workspace are converted to voxels, and a three-dimensional binary matrix of voxel occupancy is created. A corresponding distance map matrix is then computed, where each cell in the distance map matrix contains a distance to a nearest occupied cell. The distance map matrix is used as a constraint function in a motion planning optimization problem, where the optimization problem is convexified and then iteratively solved to yield a robot motion profile which avoids the obstacles and minimizes an objective function such as travel distance. A distance field optimization technique is quickly computed and has a computation time which is independent of the number of obstacles. The disclosed optimization technique is easy to set up, as it requires no creation of geometry primitives to approximate robot and obstacle shapes.SELECTED DRAWING: Figure 8

Description

Technical Field

[0001] The present disclosure relates to the field of industrial robot motion control, and more specifically, to converting obstacle data into voxels, creating a three-dimensional binary matrix of occupied voxels and unoccupied voxels, calculating a distance map including the distance to the nearest occupied cell for each cell, and then using the distance map as a constraint for motion optimization calculation to calculate the most efficient robot arm path to avoid obstacles, which is a robot collision avoidance motion planning technique.

Background Art

[0002] It is well known to widely perform manufacturing, assembly, and material movement operations using industrial robots. In many robot work space environments, there are obstacles, and those obstacles may be in the path of the robot's motion. The obstacles may be permanent structures such as machines and equipment, or they may be temporary or movable. While the robot is performing an operation such as welding, the large work piece itself being operated by the robot may become an obstacle because the robot needs to move within or around the work piece. Collisions between the robot and the obstacles must be absolutely avoided.

[0003] Conventional techniques for collision avoidance robot motion planning include defining geometric primitives such as spheres and cylinders around each arm of the robot and around each obstacle. Geometric primitives are used to reduce the complexity of the collision avoidance motion optimization calculation to a manageable level. However, defining geometric primitives around each obstacle and around each robot arm is a cumbersome and time-consuming series of operations. Furthermore, some objects such as car bodies being welded or painted by the robot are not suitable for approximation using geometric primitives.

[0004] Another problem with the prior art for collision avoidance robot motion planning is that the computational time of the optimization problem increases dramatically with the number of obstacles. When there are multiple obstacles in the robot's workspace, in an environment where it is necessary to perform collision avoidance calculations in real time during the robot's operation, the calculation of the motion plan takes too much time and is not practical.

[0005] In light of the above environment, there is a need for an improved robot motion optimization technique that is easy to set up and can quickly calculate collision avoidance robot motion regardless of the number of obstacles in the robot workspace.

Summary of the Invention

[0006] In accordance with the teachings of the present disclosure, a robot collision avoidance motion optimization technique using a distance field constraint function is disclosed. CAD or sensor data representing obstacles in the robot workspace is converted into voxels to create a three-dimensional (3D) binary matrix of voxel occupancy. Next, the corresponding 3D distance field matrix is calculated. Here, each cell of the distance field matrix contains the distance to the nearest cell occupied by an obstacle voxel. The distance field matrix is used as a constraint function for the motion plan optimization problem. Here, the optimization problem is convexified and then repeatedly solved to generate a robot motion profile that avoids obstacles and minimizes an objective function such as the travel distance. The distance field optimization technique is quickly calculated and has a computational time that does not depend on the number of obstacles. The disclosed optimization technique is easy to set up because it does not require creating geometric primitives for approximating the shapes of the robot and the obstacles.

[0007] Additional features of the presently disclosed apparatus and methods will become apparent from the following description and the appended claims in conjunction with the accompanying drawings.

Brief Description of the Drawings

[0008]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6A

Figure 6B

Figure 7

Figure 8

Figure 9

Best Mode for Carrying Out the Invention

[0009] The following considerations regarding embodiments of the present disclosure directed to robot collision avoidance motion optimization techniques using distance field constraint functions are illustrative in nature and are in no way intended to limit the disclosed apparatus and techniques or their use or application.

[0010] It is well known to use industrial robots in various manufacturing, assembly, and material handling operations. In many robot work space environments, obstacles exist, and those obstacles may be in the path of the robot's motion. That is, the obstacles may be located between the location where the robot is currently positioned and the location of the robot's destination. The obstacles may be permanent structures such as machinery and equipment, or they may be temporary or movable. While the robot is performing an operation such as welding, the large workpiece itself being operated by the robot may become an obstacle because the robot needs to move within or around the workpiece. Techniques have been developed in the art for calculating the motion of a robot so that the tool follows a path that avoids collisions with defined obstacles.

[0011] Prior art for collision avoidance robot motion planning includes defining geometric primitives such as spheres, cylinders, etc. around each arm of the robot and around each obstacle. Geometric primitives are used to reduce the complexity of the collision avoidance motion optimization calculation to a manageable level by approximating the actual object shape with a simplified shape.

[0012] FIG. 1 is a diagram of an industrial robot operating near an obstacle, where the robot and the obstacle have corresponding geometric primitives defined and used for collision avoidance calculations as used in techniques known in the art. Robot 100 is performing several operations programmed in the work space. Here, this operation can also be, for example, the movement of parts from a conveyor to a pallet or storage container, or the welding or spray painting of workpieces. Robot 100 is composed of a plurality of articulated arms including, among other things, an inner arm 110, an outer arm 120, and a wrist / tool 130. There are many obstacles in the work space that the robot 100 must avoid. Obstacles 140, 150, and 160 are examples of various types of obstacles that may exist. In many cases, obstacles 140, 150, and 160 are stationary. However, moving obstacles may also exist in the work space and can be adapted by the techniques of the present disclosure described below.

[0013] As described above, in order to simplify the collision avoidance calculation, prior art is known that defines safety zone geometric primitives around each robot arm and each obstacle. The inner arm 110 of the robot has a primitive 112, the outer arm 120 has a primitive 122, and the wrist / tool 130 has a primitive 132. Primitives 112, 122, and 132 are defined as cylinders with hemispherical ends. There are safety zone primitives defined as spheres in other parts of the robot. Primitive 112 moves with the inner arm 110 in the calculations of robot motion and collision avoidance, and the same applies to the other primitives. Similarly, obstacle 140 has a primitive 142 defined around it, obstacle 150 has a primitive 152, and obstacle 160 has a primitive 162.

[0014] Next, to calculate a robot motion that avoids interference, the distance from a robot arm primitive to an obstacle primitive is calculated. This is much easier than calculating the distance between the arm and the obstacle with their actual detailed shapes. However, defining geometric primitives around each obstacle and around each robot arm is a cumbersome and time-consuming series of operations. Further, since the actual robot arm parts and the actual obstacles typically have irregular shapes, the geometric primitives are often defined with a significant amount of empty space within the volume of the primitive. As a result, the resulting robot motion program moves the robot further than the distance actually required to avoid a collision. Further, some objects are not suitable for approximation using geometric primitives.

[0015] FIG. 2 is a view of an industrial robot 100 operating near a vehicle body 200. Here, the robot 100 has corresponding geometric primitives as shown in FIG. 1, but the vehicle body 200 is not easily modeled with respect to geometric primitives as used in the art. In the case shown in FIG. 2, in order to prevent a collision by the robot 100, it would be possible to define a hexahedron (“brick shape”) primitive around the vehicle body 200, but the hexahedron would include a large free space around the vehicle body 200. Here, the free space would prevent the robot 100 from being able to weld or paint the vehicle body 200. Further, the hexahedron would interfere with the movement of the robot 100 within the cavity or opening of the vehicle body 200. An alternative would be to define a number of geometric primitives that approximate the shape of the vehicle body 200 (for example, a cylindrical primitive for the front glass “A” pillar, another cylindrical primitive for the door “B” pillar, etc.). This would be a very cumbersome and time-consuming series of operations.

[0016] Another problem with the prior art related to conflict avoidance robot motion planning is that even when the shapes of complex robots and obstacles are approximated by geometric primitives, the computational time of the motion optimization problem increases dramatically with the number of obstacles. When there are multiple obstacles in the robot's workspace, in a real-time environment where it is necessary to continuously and quickly perform the calculation of the motion plan, it takes too much time to calculate the motion plan and is not practical.

[0017] To overcome the above problems, a new robot collision avoidance motion optimization technique using a distance field constraint function is disclosed herein. The collision avoidance motion optimization technique of the present disclosure uses computer-aided design (CAD) data or sensor data to define obstacles, avoiding the need to manually define the geometric primitives around the robot arm and obstacles, and providing an easy setup. The disclosed technique also provides fast optimization calculations, and the calculation time does not increase even when the number of obstacles increases.

[0018] FIG. 3(3A) is a diagram of obstacles in a robot workspace 302 defined as CAD data according to an embodiment of the present disclosure. Using the technique of the present disclosure, the obstacles in the robot workspace 302 can be defined by CAD data or data from an object sensor. In the example of FIG. 3(3A), CAD data is used to describe the obstacles. At the design stage of the robot operation (such as welding or painting), if the layout of the workspace 302 including the obstacles is known in advance, the CAD data may be a preferred option for defining the obstacles. If the obstacle CAD data is not available, or if it is possible to move the obstacles within the workspace 302, the data from the object sensor may be used to define the obstacles.

[0019] In FIG. 3(3A), there are obstacles 310, 320, 330, 340, 350 and 360, each having a different size and shape. The obstacles 310-360 may be any type of real part, spare part or other object, and the obstacles 310-360 are not surrounded by geometric primitives or modeled in the disclosed technology. Although most of the obstacles 310-360 are shown as simple geometric shapes, this is merely to simplify the description and make it clear. The obstacles 310~360 in FIG. 3(3A) defined by CAD data have smooth and clear surfaces. If the obstacles 310-360 are defined by object sensor data, the obstacles will include irregularities and approximations typical of object sensor data. The object sensors used to determine the obstacles 310-360 may include one or more of a digital camera, a radar sensor, a LiDAR sensor, an ultrasonic sensor and / or an infrared sensor.

[0020] FIG. 3(3B) is a diagram of the obstacles in FIG. 3(3A) converted into volume elements (voxels) according to an embodiment of the present disclosure. FIG. 3(3B) shows the same working space 302 as FIG. 3(3A), but the only difference is the voxelization of the obstacles in FIG. 3(3B). As is known in the art, a voxel represents a cell within a regular grid in three-dimensional space. Voxels are particularly useful for representing a space that is unevenly filled and periodically sampled, for example, a working space 302 that is mostly open space but has solid obstacles in some places. The obstacles 310-360 are divided into polyhedrons such as hexahedrons, or more specifically cubes, and each polyhedron is one voxel. The obstacles 310-360 are each represented as voxel sets 312-362. That is, the voxel set 312 represents the obstacle 310, the voxel set 362 represents the obstacle 360, and so on. In addition, a voxel representation of the floor 370 is provided. Converting data from macroscopic objects (from CAD or sensor data) to individual voxels is a series of operations known in the art. This series of operations is used as a preparatory step in the technology of the present disclosure.

[0021] FIG. 4 is a block diagram showing a data flow for converting obstacle CAD or sensor data into voxels, then into a binary matrix of voxel occupancy, and finally into a distance map matrix, according to one embodiment of the present disclosure. Block 410 that provides CAD or sensor data defining an obstacle and block 420 that voxelizes the obstacle were previously discussed with reference to FIGS. 3(3A) and 3(3B). Next, a three-dimensional binary matrix 430 is created from the voxels of block 420. The binary matrix 430 is a three-dimensional (3D) matrix representing the work space 302. Here, each cell within the matrix 430 has a binary value of either 0 or 1. If a cell is not occupied by any of the voxels of FIG. 3(3B), the cell within the matrix 430 has a value of 0. If any of the voxels of FIG. 3(3B) occupies some or all of a cell within the matrix 430, that cell has a value of 1.

[0022] The matrix 430 is shown in FIG. 4 as a two-dimensional grid for ease of visualization, but the matrix 430 is actually three-dimensional and represents the volume of the work space 302. For example, the 5×5 grid shown in FIG. 4 may be the bottom layer of the matrix 430. Cell 432 has a value of 1 and thus represents a voxel occupied by a portion of one of the obstacles 310-360. Additionally, cell 434 has a value of 1. Cell 436 has a value of 0 and thus represents a voxel not occupied by a portion of any of the obstacles 310-360. Additionally, cell 438 has a value of 0. Referring again to the voxelized obstacle data of block 420, all of the voxels that make up the obstacles 310-360 will have a value of 1 within the matrix 430, and all of the empty space or free space (not occupied by the obstacles 310-360) will have a value of 0 within the matrix 430. The floor 370 may be represented as a layer of (occupied) ones within the matrix 430, or the matrix 430 may be defined such that the floor 370 is below the matrix 430 and thus not included within the matrix 430.

[0023] The binary matrix 430 can have any size and dimensions suitable for the disclosed robot motion optimization purposes. In an example considered further below, the workspace 302 has a size of 1.5 meters × 1.5 meters (floor area) × 1.0 meter in height. This 1.5 × 1.5 × 1.0 meter volume may be divided into a binary matrix 430 having dimensions of 300 × 300 × 200 cells. Here, each cell is a cube having sides of 5 × 5 mm. Such dimensions are merely illustrative but represent an example that corresponds to an actual robot workspace and can be quickly computed using readily available processor hardware.

[0024] As discussed above, the binary matrix 430 is a 3D matrix of 1s and 0s representing the workspace 302. Here, 1s are the cells of the matrix 430 occupied by the obstacles 310 - 360. The next step in a series of operations is to calculate a distance map, i.e., a distance field matrix 440. The distance field matrix 440 is a 3D matrix having the same size and dimensions as the binary matrix 430. The distance field matrix 440 is also shown in FIG. 4 as a 2D grid for ease of visualization.

[0025] Each cell of the distance field matrix 440 is input with a value representing the distance from that particular cell to the nearest occupied cell. For example, consider the free space cell 436 of the binary matrix 430. The nearest occupied cell is cell 434, which is two cell units to the left. Therefore, the value of cell 446 in the distance field matrix 440 is 2.0, which means that the nearest occupied cell is 2.0 units away. Consider the free space cell 438 of the binary matrix 430. The nearest occupied cell is cell 432, which is two cell units to the right and one unit above. Therefore, the value of cell 448 in the distance field matrix 440 is 2.23 (the square root of 5, which is the length of the hypotenuse of a triangle with sides of 2 and 1), which means that the nearest occupied cell is 2.23 cell units away. This example of the distance field value is calculated in two dimensions (the square root of the sum of the squares of the distances in two directions) to match the diagram of the two-dimensional grid in Figure 4. In reality, if the distance field matrix 440 is a 3D matrix, the distance value is calculated in three dimensions (the square root of the sum of the squares of the distances in three directions). If the distance field map 440 contains values representing a large number of cells, the true distance can be obtained by multiplying by the cell size. For example, if a cell in the distance field map 440 has a value of 10.0 cells relative to the nearest occupied cell and the cell size is 5 mm, the actual distance to the nearest obstacle is 50.0 mm. Alternatively, the cells in the distance field map 440 may be input with actual distance values rather than the number of cell units.

[0026] Since cell 442 in the distance field matrix 440 corresponds to the occupied cell 432, it has a distance value of 0.0. Since cell 444 in the distance field matrix 440 is immediately adjacent (one cell away) to the occupied cell 434, it has a distance value of 1.0. Recognizing that the distance field matrix 440 is three-dimensional, it can be understood that some cells within the distance field matrix 440 are themselves occupied and surrounded by occupied cells. In the case of such cells that are "inside" the obstacles 310 - 360, a negative value is given to the distance field, and the distance field represents the distance to the nearest unoccupied cell calculated in the same three-dimensional method as previously considered (the square root of the sum of the squares of the distances in three directions).

[0027] FIG. 5 is a diagram of a robot 500 operating in a work space 502 represented by a distance field matrix 440 of FIG. 4, according to an embodiment of the present disclosure. The robot 500 is a multi-arm multi-joint robot, and each arm has one or more spherical safety zone bubbles defined around it. A bubble 510 is defined around the robot base, a bubble 512 is defined around the hip joint, bubbles 514-518 surround the inner arm, a bubble 520 surrounds the elbow joint, and bubbles 522-528 surround the outer arm and wrist / tool. The safety zone bubbles 510-528 define a spatial volume that encloses the arms of the robot 500 and will be used in the collision avoidance calculations discussed below. Each of the bubbles has a known center point and radius. For example, bubble 520 has a center point 540 and a radius 542.

[0028] It should be understood that the obstacles 310-360 shown in FIG. 3(3A) are present in the work space 502 of FIG. 5. The distance field map 440 calculated based on the obstacles 310-360 is overlaid on the work space 502 such that the robot 500 performs its work within the spatial volume of the distance field map 440. When the robot 500 moves its joints through its joint range of motion, each of the center points of the bubbles 510-528 may occupy one of the cells of the distance field matrix 440. For any given pose of the robot 500, the distance from each bubble to the closest occupied cell of the distance field map 440 can be calculated as in Equation (1).

[0029]

Equation

[0030] Here, p i is the center point of a particular one (i) of the bubbles 510-528, and r i is the radius of the bubble having the center point p i , and DF is the value of the distance field map 440 of the cell at the location of the center point p i , which is the coordinates (x i , y i , z i ).

[0031] In other words, for example, if the center point 540 of the bubble 520 is at a location (within a certain fixed working space coordinate frame) (x i , y i , z i ), the minimum distance from the bubble 520 to any of the obstacles 310 - 360 is the value of the cell in the distance field map 440 currently occupied by the center point 540 minus the radius of the bubble 520. This calculation is performed for each center point and radius of the bubbles 510 - 528 at each pose of the robot 500 evaluated during the motion optimization calculation. Since the distance field map 440 is pre-calculated for a specific obstacle field, Equation (1) is a very simple subtraction problem and can be easily solved for each of the 10 bubbles 510 - 528.

[0032] Next, for a specific pose of the robot 500, the minimum distance Dist(Robot,Obs) from the robot 500 to any of the obstacles 310 - 360 is the minimum value of the values calculated from Equation (1) for each of the 10 bubbles (i). That is, Equation (2) (Equation 2).

[0033]

Equation

[0034] For example, in a pose where the robot 500 is extended towards the obstacles 310 - 360, the bubble 528 may have the minimum distance to the obstacle because its center point occupies cells in the distance field map 440 with very low values.

[0035] When the posture of the robot 500 in which one or more of the center points are actually within an obstacle (interfering with the obstacle) is evaluated, the minimum distance will be calculated as a negative value of that posture. Since the minimum distance value is used as a constraint function that must be greater than some threshold safety value in the motion optimization calculation (discussed below), a penalty will be imposed on the posture with interference, and the motion optimization calculation will find another solution without interference.

[0036] FIG. 6A is a multi-stage view of the robot 600 following a path 610 defined without considering the collision avoidance constraints considered in the motion plan. The path 610 is defined as proceeding from the start position 612 (q start ) to the target position 614 (q goal ). It cannot be denied that the start position 612 may be a place where the tool 620 of the robot 600 takes out parts from the first storage container, and it cannot be denied that the target position 614 may be a place where the robot 600 places parts in the second storage container, for example. In FIG. 6A, the robot 600 is shown in three postures. Here, the tool 620 is arranged at the start position 612, the intermediate position, and the target position 614.

[0037] In the motion optimization calculation used to calculate a path such as the path 610, many functions can be considered. Of course, such functions are defined as equality constraints because they need to satisfy the start and target positions (612, 614). Another equality constraint including system dynamics or kinematic equations may be defined. This enables the robot to actually physically perform the calculated motion. Inequality constraints may be defined such that the rotational speed and acceleration of the joints and the Cartesian acceleration and jerk of the center point of the tool do not exceed the thresholds. Also, an optimization objective function needs to be defined. For example, the optimization objective function may be used to optimize the efficiency of the path by minimizing parameters such as the moving distance, the curvature of the path, the moving time, and the energy consumed by the robot.

[0038] If collision avoidance is not defined as a constraint function, the robot 600 is likely to follow a path that is substantially the same as path 610 through motion optimization. Here, the tool 620 of the robot 600 collides with an obstacle such as the wall 630 or the storage container 640. Since such a collision with an obstacle is clearly not acceptable, it is necessary to add a collision avoidance condition as a constraint function. In the technology of the present disclosure, the collision avoidance constraint function is defined with respect to the distance field map 440 using the calculations of equations (1) and (2).

[0039] FIG. 6B is a multi-stage view of a robot following a path defined by a collision avoidance motion plan according to an embodiment of the present disclosure. In FIG. 6B, path 650 is calculated for the tool 620 of the robot 600. Path 650 uses the same start position 612 and the same target position 614 as path 610. However, when calculating path 650, a collision avoidance constraint function is used in the motion optimization.

[0040] Specifically, a collision avoidance inequality constraint function is used in the motion optimization calculation. Here, the collision avoidance constraint is the minimum distance from the robot 600 to any of the obstacles (the wall 630 and the storage container 640) (represented by the safety zone bubble as discussed above), and the minimum distance determined from the distance field maps of the wall 630 and the storage container 640 using equations (1) and (2) must be greater than some threshold safety value (e.g., 50 mm, etc.). If this collision avoidance inequality constraint is included in the motion optimization calculation together with the other constraint functions and objective functions previously discussed with respect to FIG. 6A, the result is path 650. From FIG. 6B, it can be seen that with path 650, the tool 620 and all components of the robot 600 avoid the wall 630 and the storage container 640.

[0041] FIG. 7 is a graph 700 of distance calculation time versus the number of obstacles for a conventional distance calculation technique using geometric primitives and a distance calculation using a distance field matrix, according to an embodiment of the present disclosure. Graph 700 includes curve 710 and curve 720, each plotting distance calculation time on vertical axis 702 and the number of obstacles on horizontal axis 704. Curve 710 shows that in a conventional distance calculation technique where obstacles and a robotic arm are represented by geometric primitives, the calculation time increases dramatically with the number of obstacles. In contrast, curve 720 shows that in the distance calculation technique of the present disclosure where obstacles are represented by a distance field map and the robotic arm is represented by a number of points and spherical bubbles, the calculation time is short and independent of the number of obstacles.

[0042] When it is necessary to perform motion optimization calculations in real time during the operation of a robot, it can be easily understood that fast distance calculation using a distance field map technique is particularly advantageous. For example, consider an application where a robot needs to pick up parts from an incoming conveyor and place them in the next available location within a transport container. In this exemplary application, both the starting position (q start ) and the target position (q goal ) are unique for each pick-and-place operation. This means that it is necessary to calculate a new path using motion optimization. By using the distance field map technique to perform distance calculations from the robot to the obstacles (for the collision avoidance constraint function), iterative optimization calculations can be completed at a speed sufficient to support real-time robot operation.

[0043] FIG. 8 is a diagram of a system for optimizing robot collision avoidance motion using a distance field constraint function according to an embodiment of the present disclosure. A robot 800 operates within a work space 802 that includes one or more obstacles 810. The obstacles 810 are illustrated as the obstacles 310-360 of FIG. 3(3A) merely for the sake of consistency. A controller 820 typically communicates with the robot 800 via a wired cable connection. The controller 820 controls the motion of the robot 800 by transmitting joint motion commands to the robot 800 and receiving joint motion position data from joint encoders within the robot 800, as is known in the art.

[0044] A computer 830 that communicates with the controller 820 may be used for several different tasks, including providing obstacle shape data in the form of a CAD solid or surface model. The computer 830 communicates with the controller 820 via any suitable wireless or wired network connection when used. Instead of using CAD data to define the obstacles 810, one or more sensors such as a sensor 840 may be used. The sensor 840 may be a camera or any type of object sensor that can provide the 3D geometry of the obstacles 810 within the work space 802. The sensor 840 may be one or more 3D cameras, or multiple 2D cameras, where the data of which is combined into 3D obstacle data. The sensor 840 may additionally include other types of devices such as radar, LiDAR, and / or ultrasonic. The sensor 840 also communicates with the controller 820 and / or the computer 830 via any suitable wireless or wired network connection.

[0045] In one embodiment, the controller 820 receives obstacle shape data from either the computer 830 or the sensor 840. The obstacle shape data defines the entire 3D shape of the obstacles 810 present in the work space 802. The controller 820 voxelizes the obstacle shape data, creates a binary 3D matrix of the work space 802 that includes 1 if each cell is occupied and 0 if not, and converts the obstacle shape data into a distance field map, i.e., a matrix, by calculating a distance field map from the binary matrix. The distance field matrix, i.e., the map, may be calculated alternately by the computer 830 and provided to the controller 820. The controller 820 models the robot 800 as a set of points on the robotic arm. Here, each point has a corresponding spherical safety zone bubble of a predetermined radius. Using the bubble model of the robot 800 and the distance field map representing the obstacles 810, the controller 820 can very quickly calculate the minimum distance from the robot to the obstacles for any pose of the robot 800 using equations (1) and (2). Using the calculated minimum distance from the robot to the obstacles as a constraint function, the controller 820 performs motion optimization calculations as necessary to control the motion of the robot 800.

[0046] In another embodiment, the computer 830 performs most of the calculations, including creating the distance field matrix, modeling the motion optimization problem using the distance field constraints and other constraints, and solving the optimization problem. In this embodiment, the computer 830 provides the robot motion program to the controller 820 from the converged optimal solution. The responsibility for the calculations may be divided between the computer 830 and the controller 820 in any suitable way.

[0047] In an exemplary use, the robot 800 performs a task with continuously changing start and target positions, and thus, the motion optimization calculation needs to be calculated for each robot task. In another use, one or more obstacles 810 may be moving. In that case, it is necessary to recalculate the distance field map before the motion optimization calculation. In either use, the rapid calculation of the minimum distance from the robot to the obstacle using the distance field map technology is extremely beneficial in assisting real-time robot motion control.

[0048] FIG. 9 is a flowchart diagram 900 of a method for robot collision avoidance motion optimization using a distance field constraint function according to an embodiment of the present disclosure. In box 902, obstacles within the robot workspace are defined by CAD data or sensor data as previously described. In box 904, a distance field matrix representing the obstacles within the workspace is created. The distance field matrix is created by converting the obstacle data into a voxel model, creating a binary 3D matrix of the workspace environment where occupied cells, i.e., voxels, contain 1 and unoccupied cells, i.e., voxels, contain 0, and then calculating a distance field matrix where each cell contains a value that is the distance to the nearest occupied cell, i.e., voxel. Cells that are occupied and surrounded by other occupied cells have negative values in the distance field matrix and represent the distance to the nearest unoccupied cell.

[0049] In box 906, based on the tasks to be performed by the robot, the starting point and the target point of the robot path are defined. In box 908, an initial reference path is generated. The initial reference path may be a straight line from the starting point to the target point, or it may be an approximate path having substantially the same starting point and target point and based on a previously calculated path. The initial reference path provides a starting point (initial solution) for the motion optimization calculation. In box 910, the motion optimization problem is modeled by the robot controller. The modeling of the motion optimization problem includes the definition of the objective function and the constraint function as discussed so far. In particular, this includes that the minimum distance from the robot to any obstacle is modeled using the distance field matrix of box 904 together with the set of points on the robot arm and the spherical bubble, and includes the collision avoidance inequality constraints calculated using equations (1) and (2).

[0050] Therefore, the motion optimization problem can be defined as in equation (3) (Equation 3).

[0051]

Equation

[0052] At this time, do as follows.

[0053]

Equation

[0054]

Equation

[0055]

Equation

[0056] Here, f(q) is the posture q = {q1, ···, q T} is the optimization objective function (such as the path length of the center point of the tool) to be minimized over the entire robot motion, Equation 7 is the inequality constraint that needs to be satisfied (maintaining a state below the limit, joint limits, joint speeds, accelerations, and jerks, the speed, acceleration, and jerk of the center point of the tool, etc.), h(q) is the equality constraint that needs to be satisfied (the locations of the start and target positions, system mechanics or kinematic equations, etc.), DF(q t ) is another inequality constraint (collision avoidance), and the minimum distance from the robot to the obstacle must be greater than the threshold value d t for all postures q safe of the robot.

[0057]

Equation

[0058] Equality constraints related to system mechanics or kinematics are included to enable the robot to physically perform the calculated motion. This constraint may be modeled by including system mechanics equations in the form of Equation 8. Here, q is the angular position of the joint (and its usual first and second derivatives), M is the mass moment of inertia, C is the Coriolis coefficient, K is the stiffness, G is the load due to gravity, and τ is the joint torque.

[0059]

Equation

[0060] This type of equation will be evaluated for each joint of the robot.

[0061] Alternatively, the equality constraint may be modeled as the equation of motion in the form of a double integrator (Equation 9). Here, Equations 10 and 11 are involved.

[0062]

Equation

[0063]

Number

[0064]

Number

[0065] Also in this case, each joint of the robot will be modeled by the kinematic equations.

[0066] In box 912, the motion optimization problem is convexified in preparation for solution. Convexification of the optimization problem involves approximating one or more of the constraints as polynomial-time functions, and the resulting calculations can be solved more easily, with only one optimal solution obtained. In one embodiment, the collision avoidance inequality constraint is linearized to improve the convergence behavior of the optimization problem. In this embodiment, Equation (6) is approximated as a first-order Taylor expansion as shown in Equation (7) (Number 12) below.

[0067]

Number

[0068] Here, Number 13 is the reference position at time step t (the reference position is the initial reference given from the input or the previous optimization iteration), q t is the planned position at time step t to be solved in the optimization, and ∇DF is the gradient of the distance function DF at the position (Number 13).

[0069]

Number

[0070] The gradient terms of the linearized constraints help to "push away" the points on the robot arm from the obstacles in the distance field map.

[0071] In box 914, a convex optimization problem is solved, and the check in decision diamond 916 determines whether the solution has converged within a predetermined tolerance. Techniques for solving convex optimization problems are known in the art, and when the constraints of the collision avoidance distance field are linearized, the solution converges rapidly. In box 918, the converged optimal solution is interpolated to define a complete robot trajectory that includes the full joint motion and the tool motion that satisfies the constraint function including the collision avoidance constraint.

[0072] Considering the obstacle data definition from CAD or sensors, any of the steps of the method of flowchart 900 may be implemented by a robot controller such as controller 820 and / or computer 830. That is, in one embodiment, controller 820 calculates a distance field matrix representing obstacles and, as discussed, uses the distance field matrix and the multi-point model of the robot to calculate the distance from the robot to the obstacles for any robot pose. In this embodiment, robot controller 820 also models an optimization problem based on constraints including the starting point, the target point, the initial reference path, and the distance field matrix model, and then controller 820 solves the optimization problem and controls the movement of robot 800 in real time.

[0073] In another embodiment, robot controller 820 controls only the movement of robot 800 using an operation program calculated by computer 830. In this embodiment, computer 830 calculates a distance field matrix representing obstacles, uses the distance field matrix and the multi-point model of the robot to calculate the distance from the robot to the obstacles for any robot pose, models an optimization problem based on constraints including the starting point, the target point, the initial reference path, and the distance field matrix model, and then solves the optimization problem. As will be understood by those skilled in the art, the responsibility for the calculations may be divided in other ways. For example, computer 830 may provide the distance field matrix, and controller 820 may perform the modeling and solution of the remaining optimization problem. In some embodiments, computer 830 is not required.

[0074] Through the foregoing discussion, various computers and controllers have been described and implied. It should be understood that such software applications and modules of computers and controllers are executed on one or more computing devices having a processor and a memory module. In particular, this includes a processor in each of the robot controller 820 and the computer 830 discussed above. Specifically, the processor in the controller 820 and / or the computer 830 is configured to use the distance field matrix as a constraint function in collision avoidance optimization path planning calculations in the manner described throughout the foregoing disclosure.

[0075] As outlined above, the disclosed techniques for robot collision avoidance motion optimization techniques using distance field constraint functions improve the speed and reliability of robot path planning for collision avoidance. The disclosed techniques avoid the prior work of modeling obstacles as geometric primitives, simplify and automatically represent complex shapes as voxels, and enable them to be used in the calculation of the distance field matrix, allowing the distance from the robot to the obstacles to be calculated quickly even in the presence of multiple obstacles.

[0076] Although some exemplary aspects and embodiments of robot collision avoidance motion optimization techniques using distance field constraint functions have been discussed above, those skilled in the art will recognize modifications, substitutions, additions, and secondary combinations thereof. For this reason, the following appended claims and the claims introduced below are intended to be construed to include any such modifications, substitutions, additions, and secondary combinations that fall within their true spirit and scope.

Claims

1. 1. A method for planning a path for an industrial robot, the method comprising the steps of: providing obstacle data defining obstacles within a workspace of the robot; calculating a distance field matrix from the obstacle data, the distance field matrix being a three-dimensional (3D) matrix representing the workspace, each cell of the distance field matrix containing a value that specifies a distance to the nearest cell occupied by any portion of any of the obstacles; modeling a robot motion optimization problem using a computer having a processor and a memory, the model including: defining an objective function; defining a start point and a target point of the path as equality constraints; and defining collision avoidance inequality constraints using the distance field matrix; and solving the motion optimization problem by the computer to derive the path for the robot.

2. The method of claim 1 , wherein providing obstacle data comprises providing the obstacle data from a computer-aided design (CAD) system.

3. The method of claim 1 , wherein providing obstacle data comprises providing the obstacle data from one or more sensors configured to detect the obstacles in the workspace.

4. 2. The method of claim 1 , wherein calculating the distance field matrix from the obstacle data comprises converting the obstacle data to voxels, creating a binary 3D matrix of occupancy data, and calculating the distance field matrix from the binary 3D matrix of occupancy data.

5. 2. The method of claim 1 , wherein defining the collision avoidance inequality constraints using the distance field matrix comprises: defining a plurality of points on an arm of the robot; and for each calculated pose of the robot, determining a distance to an obstacle for each of the points as the value in the cell containing the point minus a predetermined radius around the point.

6. 6. The method of claim 5, wherein for each computed pose of the robot, a distance from the robot to an obstacle is determined as the minimum distance to the obstacle for any of the points, and the collision avoidance inequality constraint is that the distance from the robot to an obstacle must exceed a predetermined threshold.

7. The method of claim 1 , wherein the step of modeling the robot motion optimization problem further comprises generating an initial reference path based on the start point and the target point.

8. 2. The method of claim 1 , wherein modeling the robot motion optimization problem also includes defining motion limit inequality constraints including robot joint rotational velocities, accelerations, and jerks not exceeding predetermined thresholds, robot tool center point velocity, accelerations, and jerks not exceeding predetermined thresholds, and joint positions not exceeding predetermined limits, and further comprising defining equality constraints based on system dynamics or kinematic equations for each joint of the robot.

9. The method of claim 1 , further comprising the steps of approximating the collision avoidance inequality constraints as a first-order Taylor series expansion and convexifying the robot motion optimization problem prior to solving the robot motion optimization problem.

10. The method of claim 9 , wherein solving the motion optimization problem comprises iteratively solving the motion optimization problem until a solution converges to a predetermined convergence criterion.

11. The method of claim 1 , wherein solving the motion optimization problem produces a solution vector that includes the start point and the target point and a number of intermediate points, and a continuous robot trajectory is interpolated from the solution vector.

12. 1. A method for planning a path for an industrial robot, the method comprising the steps of: modeling and solving a robot motion optimization problem using a computer, wherein modeling the optimization problem comprises the steps of defining an objective function; defining a start point and a target point of the path as equality constraints; and defining collision avoidance inequality constraints using a distance field matrix, the distance field matrix being a three-dimensional matrix representing a workspace of the robot, each cell of the distance field matrix containing a value specifying a distance to the nearest cell occupied by any portion of any obstacle in the workspace.

13. 1. A path planning system for an industrial robot, the system comprising: means for providing obstacle data defining obstacles within a workspace of the robot; a computer in communication with the means for providing robot and obstacle data, the computer having a processor and a memory; calculating a distance field matrix from the obstacle data, the distance field matrix being a three-dimensional (3D) matrix representing the workspace, each cell of the distance field matrix containing a value that specifies a distance to the nearest cell occupied by any portion of any of the obstacles; modeling a robot motion optimization problem, the model comprising: defining an objective function; defining a start point and a target point of the path as equality constraints; and defining collision avoidance inequality constraints using the distance field matrix; and solving the motion optimization problem to generate the path for the robot.

14. 14. The system of claim 13, wherein providing obstacle data comprises providing the obstacle data from a computer-aided design (CAD) system or providing the obstacle data from one or more sensors configured to detect the obstacles in the workspace, the one or more sensors comprising one or more cameras, radar sensors, LiDAR sensors, ultrasonic sensors, or infrared sensors.

15. 14. The system of claim 13, wherein calculating the distance field matrix from the obstacle data comprises converting the obstacle data to voxels, creating a binary 3D matrix of occupancy data, and calculating the distance field matrix from the binary 3D matrix of occupancy data.

16. 14. The system of claim 13, wherein defining the collision avoidance inequality constraints using the distance field matrix comprises: defining a plurality of points on an arm of the robot; and for each calculated pose of the robot, determining a distance to an obstacle for each of the points as the value in the cell containing the point minus a predetermined radius of the point.

17. 17. The system of claim 16, wherein for each calculated pose of the robot, a distance from the robot to an obstacle is determined as the minimum distance to the obstacle for any of the points, and the collision avoidance inequality constraint is that the distance from the robot to the obstacle must exceed a predetermined threshold.

18. The system of claim 13 , wherein modeling the robot motion optimization problem further comprises generating an initial reference path based on the start point and the target point.

19. 14. The system of claim 13, wherein modeling the robot motion optimization problem further comprises defining motion limit inequality constraints including robot joint rotational velocities, accelerations and jerk not exceeding predetermined thresholds, robot tool center point velocity, accelerations and jerk not exceeding predetermined thresholds, joint positions not exceeding predetermined limits, and further comprising defining equality constraints based on system dynamics or kinematic equations for each joint of the robot.

20. 14. The system of claim 13, wherein the computer is further configured for convexifying the robot motion optimization problem comprising approximating the collision avoidance inequality constraints as a first order Taylor series expansion, and wherein solving the robot motion optimization problem comprises iteratively solving the motion optimization problem until a solution converges to a predetermined convergence criterion.

Images