Fast robot motion optimization with distance fields

Abstract

Robot collision avoidance motion optimization technique using distance field constraint function. CAD or sensor data depicting obstacles in a robot work cell is 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 the distance to the 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 produce a robot motion profile that avoids obstacles and minimizes an objective function such as distance traveled. The distance field optimization technique is fast to compute and has a computation time that is independent of the number of obstacles. The disclosed optimization technique is easy to set up because it does not require the creation of geometric primitives to approximate robot and obstacle shapes.

Description

TECHNICAL FIELD

[0001] The present invention relates to the field of industrial robot motion control, and more particularly to a robot collision avoidance motion planning technique that converts obstacle data into voxels, creates a three-dimensional binary matrix of occupied and unoccupied voxels, and computes a distance map from each cell to the nearest occupied cell, and then uses the distance map as a constraint in a motion optimization calculation to compute the most efficient robot arm path that avoids obstacles. BACKGROUND

[0002] It is well known to use industrial robots to perform a wide variety of manufacturing, assembly, and material movement operations. In many robotic workcell environments, obstacles exist and can be in the path of robot motion. Obstacles can be permanent structures, such as machines and fixtures, or the obstacles can be temporary or movable. Large workpieces being manipulated by the robot can themselves be obstacles, as the robot must maneuver in or around the workpiece while performing operations such as welding. Collisions between the robot and any obstacles must be absolutely avoided.

[0003] Prior art for robot collision avoidance motion planning involves defining geometric primitives, such as spheres, cylinders, etc., around each arm of the robot and around each obstacle. The geometric primitives are used in order to reduce the complexity of the collision avoidance motion optimization calculation to a manageable level. However, defining geometric primitives around each obstacle and each robot arm is a tedious and time consuming process. In addition, some objects, such as a car body being welded or painted by the robot, do not lend themselves well to approximation using geometric primitives.

[0004] Another problem with prior art for collision avoidance robot motion planning is that the computation time for the optimization problem increases significantly with the number of obstacles. If there are more than a few obstacles in the robot's workspace, the motion planning calculation takes too long to be practical in an environment where collision avoidance calculations must be performed in real time as the robot operates.

[0005] In view of the above, there is a need for an improved robot motion optimization technique that is easy to set up and quickly computes collision avoidance robot motion regardless of the number of obstacles in the robot's workspace. SUMMARY

[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 depicting obstacles in a robot's workspace is converted to voxels and a three-dimensional (3D) binary matrix of voxel occupancy is created. A corresponding 3D distance field matrix is then computed, where each cell in the distance field matrix contains the distance to the nearest cell of obstacle voxel occupancy. The distance field matrix is used as a constraint function in a motion planning optimization problem, where the optimization problem is convexified and then iteratively solved, resulting in a robot motion trajectory that avoids obstacles and minimizes an objective function such as travel distance. The distance field optimization technique is fast to compute and has a computation time that is independent of the number of obstacles. The disclosed optimization technique is easy to set up because it does not require the creation of geometric primitives to approximate the robot and obstacle shapes.

[0007] Additional features of the presently disclosed devices and methods will be apparent from the following description and appended claims, taken in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0008] Figure 1 is a schematic of an industrial robot working near obstacles, where the robot and obstacles have corresponding geometric primitives defined and used for collision avoidance computation, as in techniques known in the art,

[0009] Figure 2 is a schematic of an industrial robot working near a vehicle body, where the robot has corresponding geometric primitives as Figure 1 shown, but the vehicle body is not easily modeled in geometric primitives, as used in techniques known in the art;

[0010] Figure 3A is a schematic of obstacles in a robot's workspace defined as CAD data, in accordance with embodiments of the present disclosure;

[0011] Figure 3B is a schematic of the obstacles of Figure 3A converted to volumetric elements (voxels), in accordance with embodiments of the present disclosure;

[0012] Figure 4 is a block diagram showing the data flow for converting obstacle CAD or sensor data to voxels, then to a binary matrix of voxel occupancy, and finally to a distance field matrix, in accordance with embodiments of the present disclosure;

[0013] Figure 5 is a schematic of a robot operating in a workspace represented by the distance field matrix of Figure 4 in accordance with embodiments of the present disclosure;

[0014] Figure 6Ais a multi-step schematic of a robot following a path defined without collision avoidance constraints included in the motion planning and resulting in an obstacle collision;

[0015] Figure 6B is a multi-step schematic of a robot following a path defined with collision avoidance motion planning according to embodiments of the present disclosure;

[0016] Figure 7 is a plot of distance computation time versus number of obstacles for a conventional distance computation technique using geometric primitives and for distance computation using distance field matrices according to embodiments of the present disclosure;

[0017] Figure 8 is a schematic of a system for robot collision avoidance motion optimization using distance field constraint functions according to embodiments of the present disclosure; and

[0018] Figure 9 is a flowchart of a method for robot collision avoidance motion optimization using distance field constraint functions according to embodiments of the present disclosure. DETAILED DESCRIPTION

[0019] The following discussion of embodiments of the disclosure directed to robot collision avoidance motion optimization techniques using distance field constraint functions is merely exemplary in nature and is in no way intended to limit the disclosed devices and techniques or their application or uses.

[0020] It is well known to use industrial robots for a variety of manufacturing, assembly, and material movement operations. In many robotic workcell environments, obstacles exist and can be in the path of the robot's motion, i.e., the obstacle can be located between the robot's current location and the robot's destination location. The obstacles can be permanent structures, such as machines and fixtures, or the obstacles can be temporary or movable. Large workpieces being operated on by the robot can themselves be obstacles, as the robot must maneuver in or around the workpiece while performing operations such as welding. Techniques have been developed in the art for computing robot motion such that the tool follows a path that avoids collision of the robot with any defined obstacles.

[0021] Prior art for robot collision avoidance motion planning involves defining geometric primitives, such as spheres, cylinders, etc., around each arm of the robot and around each obstacle. The geometric primitives are used in order to reduce the complexity of collision avoidance motion optimization computations to a manageable level by approximating real object geometry with simplified shapes.

[0022] Figure 1is a schematic of an industrial robot working near obstacles, where the robot and the obstacles have corresponding geometric primitives defined and used for collision avoidance computation, as in techniques known in the art. Robot 100 performs some programmed operation in a workspace, where the operation can be, for example, moving parts from a conveyor to a tray or bin, or welding or painting a workpiece. Robot 100 is made up of multiple articulated arms, including inner arm 110, outer arm 120, and wrist / tool 130, among others. There are many obstacles in the workspace that robot 100 must avoid. Obstacles 140, 150, and 160 are examples of different types of obstacles that can be present. In many cases, obstacles 140, 150, and 160 are stationary; however, moving obstacles can also be present in the workspace and can be considered in the techniques of the present disclosure discussed below.

[0023] As mentioned above, to simplify collision avoidance computation, existing techniques define safety zone geometric primitives around each robot arm and each obstacle. Robot inner arm 110 has primitive 112, outer arm 120 has primitive 122, and wrist / tool 130 has primitive 132, where primitives 112, 122, and 132 are defined as cylinders with hemispherical ends. Other parts of the robot have safety zone primitives defined as spheres. Primitive 112 moves with inner arm 110 in robot motion and collision avoidance computation, and the same is true for the other primitives. Similarly, obstacle 140 has primitive 142 defined around it, obstacle 150 has primitive 152, and obstacle 160 has primitive 162.

[0024] Then, to compute robot motion that avoids interference, distances are computed from the robot arm primitives to the obstacle primitives, which is much simpler than computing distances between the actual detailed shapes of the arms and the obstacles. However, defining geometric primitives around each obstacle and each robot arm is a tedious and time-consuming process. Furthermore, because real robot arm parts and real obstacles often have irregular shapes, the geometric primitives are often defined with considerable empty space inside the volume of the primitive. This causes the resulting robot motion program to move the robot farther than is actually necessary to collision avoid. Furthermore, some objects cannot be well approximated with geometric primitives.

[0025] Figure 2 is a schematic of an industrial robot 100 working near a vehicle body 200, where robot 100 has corresponding geometric primitives as shown, but vehicle body 200 is not easily modeled in terms of geometric primitives, as used in techniques known in the art. Figure 1 Figure 2 ​In the case shown, a hexahedron ("brick") geometry can be defined around the vehicle body 200 to prevent collisions of the robot 100, but the hexahedron would encompass a large amount of free space around the vehicle body 200 that would prevent the robot 100 from welding or painting the vehicle body 200. In addition, the hexahedron would prevent the robot 100 from moving within cavities or openings in the vehicle body 200. An alternative is to define a large number of geometries that approximate the shape of the vehicle body 200 (e.g., a cylindrical geometry for the windshield "A" pillar, another cylindrical geometry for the vehicle door "B" pillar, etc.), which would be an overly lengthy and time-consuming process.

[0026] Another problem with prior art techniques for robot collision avoidance motion planning is that the computation time for the motion optimization problem increases significantly with the number of obstacles, even when complex robot and obstacle shapes are approximated with geometrical primitives. If there are more than a few obstacles in the robot's workspace, the motion planning computation takes too long to be practical in a real-time environment where motion planning computations must be performed continuously and quickly.

[0027] To overcome the problems described above, a new robot collision avoidance motion optimization technique is disclosed herein that uses distance field constraint functions. The collision avoidance motion optimization technique of the present disclosure provides easy setup, uses computer-aided design (CAD) data or sensor data to define obstacles, and avoids the need for manual definition of geometrical primitives around the robot arm and obstacles. The disclosed technique also provides fast optimization computation, and the computation time does not increase with the number of obstacles.

[0028] Figure 3A is a schematic diagram of obstacles in a robot workspace 302 defined as CAD data according to embodiments of the present disclosure. Using the techniques of the present disclosure, obstacles in a robot workspace 302 can be defined from CAD data or data from object sensors. In the example of Figure 3A In the example of, CAD data is used to describe the obstacles. CAD data can be a preferred option for defining obstacles when the layout of the workspace 302 including obstacles is known in advance during the design phase of a robot operation, such as welding or painting. If obstacle CAD data is not available, or if there can be moving obstacles in the workspace 302, data from object sensors can be used to define the obstacles.

[0029] In the example of, CAD data is used to describe the obstacles. CAD data can be a preferred option for defining obstacles when the layout of the workspace 302 including obstacles is known in advance during the design phase of a robot operation, such as welding or painting. If obstacle CAD data is not available, or if there can be moving obstacles in the workspace 302, data from object sensors can be used to define the obstacles. Figure 3AIn this case, there are obstacles 310, 320, 330, 340, 350, and 360, each of which has a different size and shape. Obstacles 310-360 can be any type of real component, fixture, or other object, and in the disclosed technology, obstacles 310-360 are not enclosed by a geometric primitive or modeled as a geometric primitive. Most of obstacles 310-360 are shown as simple geometric shapes, but this is merely for simplicity and clarity of illustration. Obstacles 310-360 as defined by CAD data have smooth and well-defined surfaces. If obstacles 310-360 are defined by object sensor data, they will include the typical irregularities and approximations of object sensor data. Object sensors used to determine obstacles 310-360 can include one or more of digital cameras, radar sensors, lidar sensors, ultrasonic sensors, and / or infrared sensors. Figure 3A

[0030] Figure 3B is a schematic of obstacles 310-360 converted into volumetric elements (voxels) according to embodiments of the present disclosure. Figure 3A Figure 3B The same workspace 302 is shown as in Figure 3A with the only difference being the voxelization of the obstacles in Figure 3B As is well known in the art, a voxel represents a cell in a regular grid in three-dimensional space. Voxels are particularly useful in representing a regularly sampled space that is non-uniformly filled, for example, workspace 302, which is mostly open space but has solid obstacles in certain places. Obstacles 310-360 have been divided into polyhedrons, such as hexahedrons or more specifically cubes, where each polyhedron is one voxel. Obstacles 310-360 are represented as voxel sets 312-362, respectively. That is, voxel set 312 represents obstacle 310, voxel set 362 represents obstacle 360, and so on. A voxel representation of base 370 is also provided. The conversion of data from macroscopic objects (from CAD or sensor data) into discrete voxels is a process known in the art. This process is used as a preparatory step in the technology of the present disclosure.

[0031] Figure 4 is a block diagram showing the 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 embodiments of the present disclosure. Reference is made to Figure 3A and 3B ​​discussing block 410 in which CAD or sensor data defining obstacles is provided and block 420 in which the obstacles are voxelized. A three-dimensional binary matrix 430 is then created from the voxels of block 420. Binary matrix 430 is a three-dimensional (3D) matrix representing workspace 302 in which each cell in matrix 430 has a binary value of 0 or 1. If a cell in matrix 430 is not occupied by any voxel of Figure 3B Figure 3B If any voxel of

[0032] For ease of visualization, matrix 430 is shown as a two-dimensional grid in Figure 4 but matrix 430 is actually three-dimensional and represents the volume of workspace 302. For example, Figure 4 the 5x5 grid shown in

[0033] Binary matrix 430 can have any size and dimensionality suitable for the purposes of the disclosed robot motion optimization. In one example discussed further below, workspace 302 has a size of 1.5 meters by 1.5 meters footprint by 1.0 meter high. This 1.5x1.5x1.0 meter volume can be divided in dimension into a binary matrix 430 having 300x300x200 cells, where each cell is a cube with 5x5 mm sides. These dimensions are merely exemplary, but represent an example corresponding to a real robot workspace and that can be computed quickly using readily available processor hardware.

[0034] As noted above, binary matrix 430 is a 3D matrix of 1s and 0s representing workspace 302, where 1s are cells in matrix 430 that are occupied by obstacles 310-360. The next step in the process is to compute a distance map or distance field matrix 440. Distance field matrix 440 is a 3D matrix having the same size and dimensionality as binary matrix 430. For ease of visualization, distance field matrix 440 is shown as a two-dimensional grid in​Figure 4 The occupancy map 430 is also shown as a two-dimensional grid.

[0035] Each cell in the distance field matrix 440 is populated 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 the cell 434, which is two cells in the left direction. Thus, the value of the 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 the cell 432, which is two cells in the right direction and one cell in the up direction. Thus, the value of the 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 distance field value example is computed in two dimensions (the square root of the sum of the squares of the distances in the two directions) to match the two-dimensional grid schematic of FIG. 4A. In actual practice, when the distance field matrix 440 is a 3D matrix, the distance value is computed in three dimensions (the square root of the sum of the squares of the distances in the three directions). If the distance field map 440 contains values representing the number of cells, the actual distance can be obtained by multiplying by the cell size. For example, if the value of a cell in the distance field map 440 to the nearest occupied cell is 10.0 and the cell size is 5 mm, then the actual distance to the nearest obstacle is 50.0 mm. Alternatively, the cells in the distance field map 440 can be populated with actual distance values rather than the number of cell units. Figure 4

[0036] The cell 442 in the distance field matrix 440 has a distance value of 0.0 because it corresponds to the occupied cell 432. The cell 444 in the distance field matrix 440 has a distance value of 1.0 because it is immediately next to (one cell away from) the occupied cell 434. Recognizing that the distance field matrix 440 is three-dimensional, it can be understood that some cells in the distance field matrix 440 are themselves occupied by occupied cells and surrounded by those occupied cells. For cells such as these, which are "inside" one of the obstacles 310-360, the distance field is assigned a negative value and represents the distance to the nearest unoccupied cell, which is computed in the same three-dimensional manner (the square root of the sum of the squares of the distances in the three directions) as previously discussed.

[0037] Figure 5 is in accordance with an embodiment of the present disclosure. Figure 4 ​Schematic diagram of robot 500 operating in workspace 502 represented by distance field matrix 440. Robot 500 is a multi-arm articulated robot, and each arm has one or more spherical safe zone bubbles defined around it. Bubble 510 is defined around the robot base, bubble 512 is defined around the hip joint, bubbles 514-518 around the inner arm, bubble 520 around the elbow joint, and bubbles 522-528 around the outer arm and wrist / tool. Safe zone bubbles 510-528 define a volume of space surrounding the arms of robot 500 and will be used in the collision avoidance calculations discussed below. Each bubble has a known center point and radius. For example, bubble 520 has center point 540 and radius 542.

[0038] I understand. Figure 3A Obstacles 310-360 are shown to exist in Figure 5 The distance field map 440 calculated based on the obstacles 310-360 is superimposed on the workspace 502 so that the robot 500 performs its work within the spatial volume of the distance field map 440. As the robot 500 articulates within its range of motion, each center point of the bubbles 510-528 can occupy a cell of the distance field matrix 440. At any particular pose of the robot 500, the distance from each bubble to the nearest occupied cell of the distance field map 440 can be calculated as:

[0039] Dist(p i , r i , DF)=DF(x i ,y i , z i )-r i (1)

[0040] where p i is the center point of a particular one (i) of bubbles 510-528, r i is a point with a center point p i The radius of the bubble, DF is at the center point p i The coordinates of the position (x i ,y i , z i ) is the value of the distance field map 440 for the cell at ).

[0041] In other words, when the center point 540 of the bubble 520 is at a position (x i ,y i , z i) then 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 each bubble 510-528 at each pose of the robot 500 evaluated during the motion optimization calculation. Because the distance field map 440 is pre-computed for a particular field of obstacles, equation (1) is a very simple subtraction problem that is easily solved for each of the ten bubbles 510-528.

[0042] Then, for a particular 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 of the individual values computed for each of the ten bubbles (i) according to equation (1). That is:

[0043]

[0044] For example, in a pose where the robot 500 is heading down to an obstacle 310-360, the bubble 528 can have the minimum distance to the obstacle due to its center point occupying a cell in the distance field map 440 with a very low value.

[0045] If a pose of the robot 500 where one or more of its center points are actually inside an obstacle (interfering obstacle) is evaluated, the minimum distance will be computed as a negative value for that pose. Because 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), the interfering pose will be penalized and will cause the motion optimization calculation to find a different solution without the interference.

[0046] Figure 6A is a multi-step illustration of a robot 600 along a path 610 defined without collision avoidance constraints considered in motion planning. The path 610 is defined to proceed from a start position 612 (q start ) to a goal position 614 (q goal ). For example, the start position 612 can be a position where a tool 620 of the robot 600 picks up a part from a first bin, and the goal position 614 can be a position where the robot 600 places the part in a second bin. In Figure 6A the robot 600 is shown in three poses; with the tool 620 in the start position 612, an intermediate position, and the goal position 614.

[0047] Many factors can be considered in the motion optimization computation for computing a path such as path 610. Of course, the start and goal positions (612, 614) must be satisfied, so these are defined as equality constraints. Another equality constraint can be defined that includes the system dynamics or kinematics equations that ensure that the computed motion can actually be physically executed by the robot. Inequality constraints can be defined to prevent joint rotational velocities and accelerations as well as tool center point Cartesian accelerations and jerk from exceeding threshold values. Furthermore, an optimization objective function must be defined, and this optimization objective function can be used to optimize the efficiency of the path by minimizing parameters such as traveled distance, path curvature, travel time, or energy consumed by the robot.

[0048] If collision avoidance is not defined as a constraint function, then the motion optimization will likely cause the robot 600 to follow a path similar to path 610 in which the tool 620 of the robot 600 collides into an obstacle such as wall 630 or bin 640. Collision with any such obstacle is obviously unacceptable, so a collision avoidance condition must be added as a constraint function. In the techniques of the present disclosure, the computation using equations (1) and (2) defines a collision avoidance constraint function from the distance field map 440.

[0049] Figure 6B is a multi-step schematic of a robot following a path defined with collision avoidance motion planning according to embodiments of the present disclosure. In Figure 6B A path 650 is computed for the tool 620 of the robot 600. Path 650 uses the same start position 612 and the same goal position 614 as path 610. However, in computing path 650, a collision avoidance constraint function is used in the motion optimization.

[0050] In particular, a collision avoidance inequality constraint function is used in the motion optimization computation in which the collision avoidance constraint is defined such that the minimum distance from the robot 600 (represented by the safety zone bubble as described above) to any one of the obstacles (wall 630 and bin 640) must be greater than some threshold safety value (e.g., such as 50 mm), the minimum distance being determined from the distance field map of the wall 630 and bin 640 using equations (1) and (2). When the motion optimization computation includes this collision avoidance inequality constraint, along with the other constraint functions and objective functions discussed above with reference to Figure 6A path 650 is obtained. As can be seen in Figure 6B path 650 causes the tool 620 and all components of the robot 600 to avoid the wall 630 and bin 640.

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

[0052] It can be readily appreciated that fast distance calculations using distance field mapping techniques are particularly advantageous when motion optimization calculations must occur in real time during robot operation. Consider, for example, an application where a part on an inbound conveyor must be picked up by a robot and placed at the next available location in a shipping container. In this example application, the starting position (q start ) and target position (q goal ) is unique for each pick and place operation, which means that a new path must be calculated using motion optimization. When the robot-to-obstacle distance calculations (for the collision avoidance constraint function) are performed using distance field mapping techniques, the iterative optimization calculations can be completed quickly enough to support real-time robot operation.

[0053] Figure 8 8 is a schematic 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 in a workspace 802 including one or more obstacles 810. For consistency, the obstacles 810 are shown as Figure 3A The controller 820 typically communicates with the robot 800 via a hard-wired cable connection. As is known in the art, the controller 820 controls the motion of the robot 800 by sending joint motor commands to the robot 800 and receiving joint motor position data from joint encoders in the robot 800.

[0054] A computer 830 in communication with the controller 820 can be used for several different tasks, including providing obstacle geometry data in the form of a CAD solid or surface model. If used, the computer 830 communicates with the controller 820 via any suitable wireless or hardwired network connection. As an alternative to using CAD data to define the obstacles 810, one or more sensors, such as sensor 840, can be used. The sensor(s) 840 can be a camera or any type of object sensor capable of providing 3D geometry of the obstacles 810 in the workspace 802. The sensor(s) 840 can be one or more 3D cameras, or multiple 2D cameras whose data is combined into 3D obstacle data. The sensor(s) 840 can also include other types of devices such as radar, lidar, and / or ultrasound. The sensor(s) 840 also communicate with the controller 820 and / or the computer 830 via any wireless or hardwired network connection.

[0055] In one embodiment, the controller 820 receives obstacle geometry data from the computer 830 or the sensor(s) 840. The obstacle geometry data defines the 3D shape of all obstacles 810 present in the workspace 802. The controller 820 converts the obstacle geometry data into a distance field map or matrix by voxelizing the obstacle geometry data, creating a binary 3D matrix where each cell contains a 1 if occupied and a 0 if unoccupied of the workspace 802, and computing a distance field map from the binary matrix. The distance field matrix or map can alternatively be computed by the computer 830 and provided to the controller 820. The controller 820 models the robot 800 as a set of points on the robot arm, where each point has a corresponding predefined radius spherical safety zone bubble. Using the bubble model of the robot 800 and the distance field map representing the obstacles 810, the controller 820 can use equations (1) and (2) to compute the minimum robot-to-obstacle distance very quickly for any pose of the robot 800. Using the computed minimum robot-to-obstacle distance as a constraint function, the controller 820 performs the motion optimization computations needed to control the operation of the robot 800.

[0056] In another embodiment, the computer 830 performs most of the computations, 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 from the converged optimization solution to the controller 820. The computational responsibilities can be divided between the computer 830 and the controller 820 in any suitable manner.

[0057] In one example application, the robot 800 performs a task that involves continuously changing the start and goal positions, and thus requires the computation of motion optimization for each robot task. In another application, one or more obstacles 810 can be moving, in which case the distance field map must be recomputed prior to the motion optimization computation. In either application, the use of distance field map techniques to quickly compute the minimum robot-to-obstacle distance is extremely beneficial in supporting real-time robot motion control.

[0058] Figure 9 is a flowchart 900 of a method of robot collision avoidance motion optimization using distance field constraint functions according to embodiments of the present disclosure. At block 902, obstacles in the robot workspace are defined from CAD data or sensor data, as previously described. At block 904, a distance field matrix representing the obstacles in 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 or voxels contain a 1 and unoccupied cells or voxels contain a 0, and then computing a distance field matrix where each cell contains a value that is the distance to the nearest occupied cell or voxel. Cells that are occupied and surrounded by other occupied cells have negative values in the distance field matrix to represent the distance to the nearest unoccupied cell.

[0059] At block 906, a start point and a goal point for the robot path are defined based on the task to be performed by the robot. At block 908, an initial reference path is generated. The initial reference path can be a straight line from the start point to the goal point, or it can be an approximate path based on previously computed paths with similar start and goal points. The initial reference path provides a starting point (initial solution) for the motion optimization computation. At block 910, the motion optimization problem is modeled in the robot controller. Modeling the motion optimization problem includes defining the objective function and the constraint functions as previously described. In particular, this includes the collision avoidance inequality constraint, where the distance field matrix of block 904 is used, along with a set of points and spherical bubbles on the robot arm, to model the minimum distance from the robot to any obstacle, and equations (1) and (2) are used to compute this minimum distance.

[0060] Thus, the motion optimization problem can be defined as:

[0061]

[0062] such that

[0063] g(q) < 0 (4)

[0064] h(q) = 0 (5)

[0065]

[0066] Where f(q) is the number of pixels in the pose q={q1, .., q T} is the optimization objective function to be minimized over the entire robot motion (e.g., the path length of the tool center point), g(q) is the inequality constraint that must be satisfied (e.g., joint limits, joint velocities, accelerations, and jerks, and tool center point velocities, accelerations, and jerks must be kept below limits), h(q) is the equality constraint that must be satisfied (e.g., the positions of the start and goal points, and the system dynamics or kinematics equations), DF(q t ) is another inequality constraint (collision avoidance), where for all robot poses q t , the minimum distance from the robot to the obstacle must be greater than the threshold value d safe , as discussed in detail above.

[0067] Include equality constraints related to the system dynamics or kinematics to ensure that the calculated motion can actually be performed physically by the robot. This constraint can be modeled by including system dynamics equations of the form, -where q is the joint angular position (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 gravity load, and τ is the joint torque. These equations are evaluated for each joint of the robot.

[0068] Alternatively, the equality constraints can be modeled as the kinematic equations in double integrator form,

[0069]

[0070] in: and

[0071] Again, each joint in the robot is modeled using kinematic equations.

[0072] At block 912, the motion optimization problem is convexified in preparation for solution. Convexifying the optimization problem involves approximating one or more constraints as polynomial time functions, resulting in computations that are easier to solve and produce only one optimal solution. In one embodiment, the collision avoidance inequality constraints are linearized to improve the convergence behavior of the optimization problem. In this embodiment, equation (6) is approximated as a first-order Taylor expansion as follows:

[0073]

[0074] in 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 being solved in the optimization, is in position The gradient of the distance function DF at the point. The gradient term of the linearized constraint is used to "push" the point on the robot arm away from the obstacle in the distance field.

[0075] At block 914, the convexified optimization problem is solved, and a check is made at decision diamond 916 to determine if the solution converged within a predetermined tolerance. Techniques for solving convexified optimization problems are known in the art, and in the case of collision distance field constraint linearization, the solution converges rapidly. At block 918, the converged optimization solution is interpolated to define a complete robot trajectory including all joint motions and tool motions that satisfy the constraint functions including the collision constraints.

[0076] Given obstacle data definition from CAD or sensors, all steps of the method of flowchart 900 can be performed in a robot controller such as controller 820 and / or computer 830. That is, in one embodiment, controller 820 computes the distance field matrix representing the obstacles and uses the distance field matrix and a multi-point model of the robot in question to compute the robot-to-obstacle distance for any robot pose. In this embodiment, robot controller 820 also models the optimization problem based on the start and goal points and the initial reference path, the distance field matrix model, and other constraints, and controller 820 then solves the optimization problem and controls the motion of robot 800 in real time.

[0077] In another embodiment, robot controller 820 controls the motion of robot 800 using only the motion program computed by computer 830. In this embodiment, computer 830 computes the distance field matrix representing the obstacles, uses the distance field matrix and a multi-point model of the robot to compute the robot-to-obstacle distance for any robot pose, models the optimization problem based on the start and goal points and the initial reference path, the distance field matrix model, and other constraints, and then solves the optimization problem. As will be appreciated by those skilled in the art, the computational responsibilities can also be divided in other ways. For example, computer 830 can provide the distance field matrix, and controller 820 can perform the rest of the optimization problem modeling and solving. In some embodiments, computer 830 is not needed.

[0078] Throughout the foregoing discussion, various computers and controllers are described and implied. It will be appreciated that the software applications and modules of these computers and controllers are executed on one or more computing devices having processors and memory modules. In particular, this includes the processors in each of the aforementioned robot controller 820 and computer 830. Specifically, the processors in controller 820 and / or computer 830 are configured to use the distance field matrix as a constraint function in collision-avoiding optimization path planning computations in the manner described throughout the foregoing disclosure.

[0079] As described above, the disclosed robot collision avoidance motion optimization techniques using distance field constraint functions improve the speed and reliability of collision avoidance robot path planning. The disclosed techniques avoid the upfront effort of modeling obstacles as geometric primitives, allow complex shapes to be easily and automatically represented as voxels and used to compute distance field matrices, and enable fast computation of robot-to-obstacle distances even when there are many obstacles.

[0080] While several example aspects and embodiments of collision avoidance motion optimization techniques using distance field constraint functions have been discussed above, one of ordinary skill in the art will appreciate a variety of modifications, permutations, additions and subcombinations of the above-discussed aspects and embodiments. Therefore, the appended claims and claims hereafter introduced are intended to encompass all such modifications, permutations, additions, and subcombinations, as are within their true spirit and scope.

Claims

1. A method for planning a path of an industrial robot, the method comprising: providing obstacle data, the obstacle data defining respective obstacles in a workspace of the robot; calculating a distance field matrix based on the obstacle data, the distance field matrix being a three-dimensional (3D) matrix representing the workspace, wherein each cell in the distance field matrix contains a value indicating a distance to a nearest cell occupied by any portion of any of the respective obstacles; Modeling a robot motion optimization problem using a computer having a processor and a memory, including defining an objective function, defining a start point and a destination point of the path as equality constraints, and defining a collision avoidance inequality constraint using the distance field matrix; Modeling the robot motion optimization problem further includes generating an initial reference path based on the starting point and the target point; and solving the motion optimization problem by the computer to generate the path of the robot.

2. The method according to claim 1, wherein Providing obstacle data includes providing the obstacle data from a computer-aided design (CAD) system.

3. The method according to claim 1, wherein Providing obstacle data includes providing the obstacle data from one or more sensors configured to detect the respective obstacles in the workspace.

4. The method according to claim 1, wherein Computing the distance field matrix from the obstacle data includes converting the obstacle data into voxels, creating a binary 3D matrix of occupancy data, and computing the distance field matrix from the binary 3D matrix of occupancy data.

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

6. The method according to claim 5, wherein: For each calculated pose of the robot, a robot-to-obstacle distance is determined as the minimum-to-obstacle distance of any of the plurality of points, and the collision avoidance inequality constraint is that the robot-to-obstacle distance must exceed a predetermined threshold value.

7. The method according to claim 1, wherein Modeling the robot motion optimization problem also includes defining motion limit inequality constraints, and also includes defining equality constraints based on the system dynamics or kinematic equations of each joint of the robot, wherein the motion limit inequality constraints include that the robot joint rotation speed, acceleration and jerk do not exceed predetermined threshold values, the robot tool center point speed, acceleration and jerk do not exceed predetermined threshold values, and the joint position does not exceed predetermined limits.

8. The method of claim 1, further comprising convexifying the robot motion optimization problem before solving the robot motion optimization problem, comprising approximating the collision avoidance inequality constraint as a first-order Taylor series expansion.

9. The method according to claim 8, wherein Solving the motion optimization problem includes iteratively solving the motion optimization problem until a solution converges to a predetermined convergence criterion.

10. The method according to claim 1, wherein Solving the motion optimization problem generates a solution vector including the starting point, the target point and several intermediate points, and interpolating continuous robot trajectories according to the solution vector.

11. A method for planning a path for an industrial robot, the method comprising using a computer to model and solve a robot motion optimization problem, wherein modeling the optimization problem comprises defining an objective function, defining a start point and a destination point of the path as equality constraints, and defining collision avoidance inequality constraints using a distance field matrix, and modeling the robot motion optimization problem further comprises generating an initial reference path based on the start point and the destination point; the distance field matrix is ​​a three-dimensional matrix representing a workspace of the robot, wherein each cell in the distance field matrix contains a value indicating the distance to the nearest cell occupied by any portion of any obstacle in the workspace.

12. A path planning system for an industrial robot, the system comprising: means for providing obstacle data defining respective obstacles in a workspace of the robot; as well as a computer in communication with the robot and the means for providing obstacle data, the computer having a processor and a memory configured to; calculating a distance field matrix based on the obstacle data, the distance field matrix being a three-dimensional (3D) matrix representing the workspace, wherein each cell in the distance field matrix contains a value indicating a distance to a nearest cell occupied by any portion of any of the respective obstacles; Modeling a robot motion optimization problem, including defining an objective function, defining a start point and a target point of the path as equality constraints, and defining a collision avoidance inequality constraint using the distance field matrix; Modeling the robot motion optimization problem further includes generating an initial reference path based on the starting point and the target point; and The motion optimization problem is solved to generate the path of the robot.

13. The system according to claim 12, wherein: Providing obstacle data includes 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 respective obstacles in the workspace, wherein the one or more sensors include one or more cameras, radar sensors, lidar sensors, ultrasonic sensors, or infrared sensors.

14. The system according to claim 12, wherein: Computing the distance field matrix from the obstacle data includes converting the obstacle data into voxels, creating a binary 3D matrix of occupancy data, and computing the distance field matrix from the binary 3D matrix of occupancy data.

15. The system according to claim 12, wherein: Defining the collision avoidance inequality constraint using the distance field matrix includes defining a plurality of points on the arm of the robot, and determining, for each calculated pose of the robot, a distance to an obstacle for each of the plurality of points as a value in a cell containing the point minus a predetermined radius of the point.

16. The system according to claim 15, wherein: For each calculated pose of the robot, a robot-to-obstacle distance is determined as the minimum-to-obstacle distance of any of the plurality of points, and the collision avoidance inequality constraint is that the robot-to-obstacle distance must exceed a predetermined threshold value.

17. The system of claim 12, wherein: Modeling the robot motion optimization problem also includes defining motion limit inequality constraints, and also includes defining equality constraints based on the system dynamics or kinematic equations of each joint of the robot, wherein the motion limit inequality constraints include that the robot joint rotation speed, acceleration and jerk do not exceed predetermined threshold values, the robot tool center point speed, acceleration and jerk do not exceed predetermined threshold values, and the joint position does not exceed predetermined limits.

18. The system of claim 12, wherein: The computer is further configured to convexify the robot motion optimization problem, including approximating the collision avoidance inequality constraint as a first-order Taylor series expansion, and wherein solving the robot motion optimization problem includes iteratively solving the motion optimization problem until a solution converges to a predetermined convergence criterion.

Images