Collision map generation and planning

Abstract

To provide an object interference check technique using a collision map.SOLUTION: The CAD model of the workpiece is converted into an obstacle height map defining, for each cell of the grid, a height of the obstacle above a reference plane. The CAD model of the tool is used to construct a tool height map. The obstacle height map and the tool height map are used to generate a collision map defining the height of a non-colliding tool above each grid cell of the collision map. The collision map is generated by scanning the tool height map over the obstacle height map and determining, for each scanned point, a height of the tool height map that does not result in interference between any pixel of the tool height map and a corresponding pixel of the obstacle height map. Collision checking of the tool trajectory is then performed by comparing the trajectory check points to the grid cells of the collision map.SELECTED DRAWING: Figure 10

Description

[Technical Field]

[0001] The present disclosure relates to the field of motion control of industrial machines, and more particularly to an object interference checking technique that uses an obstacle height map and a tool height map to generate a collision map that defines a non-colliding tool height above each grid cell of the collision map, and then performs collision checking of the tool trajectory by comparing trajectory check points with the grid cells of the collision map. [Background technology]

[0002] The use of multi-axis machine tools to perform a wide range of operations, such as milling and drilling, is well known. In the workspace environment of many machine tools, obstacles may be present and in the path of the tool's motion. The obstacles may be permanent structures, such as fixtures, present in the workspace. Because the machine tool must operate on or around a workpiece while moving from one machining operation to the next, the workpiece being manipulated by the machine tool may itself be an obstacle. Collisions between any part of the machine tool and any obstacles must be reliably avoided.

[0003] It is known to include interference checking algorithms in motion planning routines, including during real-time motion planning. One conventional technique for interference checking involves defining geometric primitives, such as spheres and cylinders, around a portion of the machine tool and around each obstacle. The geometric primitives are used to reduce the complexity of the interference checking calculations to a manageable level so that the calculations can be performed quickly enough for real-time motion planning. However, defining geometric primitives around each obstacle and portion of the machine tool is a tedious and time-consuming process. Furthermore, most workpieces manipulated by machine tools are poorly suited to approximation using geometric primitives. A conservative approximation of the geometric primitive shapes can lead to false-positive interference conditions.

[0004] Another conventional technique for interference checking uses CAD models of machine tools and obstacles. Using CAD models in interference check calculations avoids the problem of inaccuracies in geometric primitives. However, this method requires calculating distances and detecting interferences between every position in a CAD model (e.g., a tool) and every position in the CAD models of every other possible obstacle in the workspace (including fixtures, workpieces, etc.). This calculation is computationally intensive and slow for all but the simplest workspace environments. In most cases, collision avoidance calculations take too long to be practical in situations where motion planning calculations need to be performed in real time as the machine tool operates. Signed distance function (SDF) techniques are known that simplify the geometry to a 3D grid used for distance or interference calculations, but this method still involves significant computational complexity.

[0005] Other interference checking techniques are also known in the art, including the axis-aligned bounding box (AABB) tree method, which covers the object body with a bounding box and checks to determine whether the boxes overlap. This method also suffers from inaccuracies due to approximations and can involve complex setups and long computation times. Summary of the Invention [Problem to be solved by the invention]

[0006] In view of the above, there is a need for improved object interference checking techniques that are easy to configure and provide fast and accurate calculations of potential object interferences, including for use in real-time machine tool collision avoidance path planning. [Means for solving the problem]

[0007] According to the teachings of the present disclosure, an object interference checking technique using a collision map is disclosed. A CAD model of a workpiece or other obstacle is converted into an obstacle height map that defines, for each cell of a grid, the height of the obstacle above the plane of a reference coordinate system. A CAD model of a tool is similarly used to construct a tool height map relative to the tool center point. The obstacle height map and the tool height map are used to generate a collision map that defines a non-colliding tool height above each grid cell of the collision map. The collision map is generated by scanning the tool height map over an area of ​​the obstacle height map and, for each scan point location, determining the tool height map height that does not result in interference between any pixel of the tool height map and the corresponding pixel of the obstacle height map. Collision checking of the tool trajectory is then performed by comparing the trajectory check points with the grid cells of the collision map.

[0008] Additional features of the apparatus and methods of the present disclosure will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings. [Brief explanation of the drawings]

[0009] [Figure 1] FIG. 1 is a schematic diagram of a system including a computer-controlled machine tool for performing machining operations on a workpiece, of a type applicable to the techniques of this disclosure. [Figure 2] FIG. 2 is a height map diagram for a simple object illustrating how height above a reference plane is defined for each cell of a grid projected onto the reference plane, according to an embodiment of the present disclosure. [Figure 3] FIG. 3 is an illustrative flow diagram of a method for creating a height map of a workpiece that is an obstacle to a machine tool path, according to an embodiment of the present disclosure. [Figure 4] FIG. 4 is an illustrative flow diagram of a method for using the height map of FIG. 3 to perform collision checking of a tool center point path and to define a new collision-free path based on the detected collision using the height map, according to an embodiment of the present disclosure. [Figure 5] FIG. 5 is a flowchart diagram of a method for performing collision checking of a tool center point path and defining a new collision-free path based on detected collisions using a height map according to an embodiment of the present disclosure. [Figure 6] FIG. 6 is a diagram of a technique for interference checking individual path points relative to a machine tool center point path using an obstacle height map, and a flowchart diagram of corresponding method steps, according to an embodiment of the present disclosure. [Figure 7] FIG. 7 is a diagram of a machine tool operating in an environment of workpiece obstacles, depicting how a portion of the tool body or holder may collide with the obstacle if only tool center point interference checking is performed, according to an embodiment of the present disclosure. [Figure 8] FIG. 8 is an illustration of a machine tool and corresponding tool height map used in a map-to-map interference checking technique, according to an embodiment of the present disclosure. [Figure 9] FIG. 9 is an illustrative flow diagram of a method for performing collision checking of a machine tool path using an obstacle height map and a tool height map and defining a new collision-free path based on detected collisions using the height maps, according to an embodiment of the present disclosure. [Figure 10] FIG. 10 is a diagram of a technique for collision checking path points of a machine tool path using an obstacle height map and a tool height map, and a flowchart diagram of corresponding method steps, in which each pixel in the tool height map is collision checked for each path point, according to an embodiment of the present disclosure. [Figure 11] FIG. 11 is a flowchart diagram of a method for performing interference checking between a tool and workspace obstacles, including pre-generating a workspace collision map and using the collision map for rapid tool path interference checking, according to an embodiment of the present disclosure. [Figure 12] FIG. 12 is an illustration of a technique for reconstructing a tool height map with radial margins according to an embodiment of the present disclosure. [Figure 13]FIG. 13 is an illustration of a technique for creating a workspace collision map by scanning a tool height map over an obstacle height map according to an embodiment of the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0010] The following description of embodiments of the present disclosure directed to object interference checking using height maps and collision maps is merely exemplary in nature and is in no way intended to limit the apparatus and techniques of the present disclosure or their applications or uses.

[0011] The use of multi-axis machine tools to perform a wide range of machining operations, such as milling and drilling, is well known. In many machine tool workspace environments, obstacles may be present and in the path of the tool's motion. The obstacles may be permanent structures, such as fixtures, present in the workspace. Because the machine tool must operate on or around a workpiece while moving from one machining operation to the next, the workpiece being manipulated by the machine tool may itself be an obstacle. Collisions between any part of the machine tool and any obstacles must be reliably avoided. This disclosure describes techniques for rapid collision checking of motion plans for applications such as these.

[0012] FIG. 1 is a schematic diagram of a system 100 including a computer-controlled machine tool that performs machining operations on a workpiece, of a type applicable to the technology of the present disclosure. The machine tool 110 rotates a spindle 112 to which a cutting tool 120 is fixed. The machine tool 110 causes the cutting tool 120 to perform machining operations on a workpiece 130. The machine tool 110 communicates with a controller 140, which is a computing device that provides motion commands and spindle motor speed commands to the machine tool 110. In a typical example, the machine tool 110 moves the rotary cutting tool 120 from a starting point along a path that removes material from the workpiece 130, and then moves the cutting tool 120 away from the workpiece 130. Before performing the next cutting operation, the machine tool 110 must move the cutting tool 120 to the start point of the next operation; this movement between cutting operations is known as an air-cutting operation. Machining operations may include, for example, milling, drilling, or tapping.

[0013] As described in detail below, the techniques of the present disclosure are applicable to the system 100 of FIG. 1 , and in particular to calculating collision-free paths for air-cutting processes between machining operations. The collision avoidance method of the present disclosure using height maps and collision maps can be programmed in the controller 140 using data available to the controller 140 or readily provided to the controller 140. Portions of the interference checking calculations described below can also be programmed in a separate computer in communication with the controller 140. Although the height map and collision map interference checking calculations are described below in terms of a collision avoidance application for air-cutting processes, the same techniques can be applied to cutting processes in which the thickness of material removal can be calculated using the height map and collision map.

[0014] The elements of Figure 1 are depicted in an overly simplified manner, where machine tool 110 is movable in three primary axes of motion, including a "vertical" direction (parallel to the axis of cutting tool 120) and two "horizontal" directions (orthogonal to the axis of cutting tool 120). However, the techniques of this disclosure are not limited to application to vertical and horizontal directions as shown in Figure 1 and subsequent figures. It should be understood that the height map and collision map collision avoidance methods of this disclosure are applicable to any type of machine tool where a collision-free path of a tool relative to a workpiece or other obstacle needs to be calculated, including multi-axis machines with full tool placement and spatial orientation capabilities, and robotically controlled mills and drills with articulated robotic arms that offer full tool placement and orientation flexibility.

[0015] 2 is a height map diagram for a simplified object, depicting how a height above a reference plane is defined for each cell of a grid projected onto the reference plane, according to an embodiment of the present disclosure. The object 200 is placed in a workspace in which a cutting tool 210 performs an operation. The cutting tool 210 is shown as a simplified thin cylinder representing a tool such as an end mill. It should be understood that the entire machine tool is also present in the workspace, as shown in FIG. 1. As also shown in FIG. 1, the cutting tool 210 has a tool center point 212, and the path of the tool center point 212 is controlled by a machine controller.

[0016] Object 200 has a very simple shape, as if it were created by stacking and joining together 14 smaller cubes as shown. Cubes 220 and 222 (and several others not numbered) are on the bottom layer and have nothing above them. Cube 224 (and others) are on top of the bottom cubes and have nothing above them. Cube 226 is on top of two other cubes. Each cube visible in FIG. 2 has a number attached to its top surface. This value represents the height of the object above a grid attached to reference plane 230. Reference plane 230 is the XY plane of reference coordinate system 232.

[0017] A height map 250 is shown on the right corresponding to object 200. Height map 250 is defined by a grid in reference plane 230 (i.e., the XY plane), where each grid cell contains a numerical value that indicates the height of the object at that grid cell. For example, grid cell 252 has a height value of 3, which corresponds to cube 226 of object 200 shown on the left. The correlation between the grid cell values ​​in height map 250 and the height of each portion of object 200 is clearly discernible.

[0018] Tool center point path 260 passes over height map 250. Tool center point path 260 represents the path of tool center point 212 as it moves over object 200. Tool center point path 260 starts at point 262 outside the height map grid and enters height map 250 on grid cell 252. Therefore, when expressed in coordinates of reference coordinate system 232, it is known that tool center point path 260 must have a height value (Z coordinate) greater than 3 when path 260 is on grid cell 252. Path 260 then passes over two grid cells with a height value of 2, and then over two more grid cells with a height value of 1. The height value (Z coordinate) of tool center point path 260 is checked against the height map value for each grid cell that path 260 passes over.

[0019] FIG. 2 is provided as an illustration of the basic concept of height maps, using them to check for collisions with path-following tools. The blocky shape and integer height values ​​of object 200 are merely for simplicity and clarity. In actual implementations, height maps can be created for objects of any arbitrary shape, where the height map value contained in each grid cell is the object's maximum height above a reference plane within the grid cell's vertical projection. The height values ​​can be defined to any desired level of precision (units and significant digits). This type of application is shown in some of the following figures and described in more detail below. Also, the orientation of the height map's Z axis need not be vertical in actual world coordinates; the height map's reference coordinate system can have any arbitrary orientation as desired.

[0020] FIG. 3 is an illustrative flow diagram of a method for creating a height map of a workpiece that is an obstacle to a machine tool path, according to an embodiment of the present disclosure. When the workpiece 300 is defined in a computer-aided design (CAD) system, the workpiece 300 is shown in the upper left in the form of a solid model or a surface model. Given a CAD model of the workpiece 300, a height map is created for the workpiece 300 in step 310 using the techniques described above with reference to FIG. 2. That is, a reference coordinate system is defined, and the CAD model of the workpiece 300 is positioned in a suitable manner in the reference coordinate system. In a typical application, the reference coordinate system is defined in its XY plane, which represents the top surface of a table or fixture to which the workpiece 300 is fixed in the workspace. Grid cell sizes in the X and Y directions (which may be the same or different) are defined. Then, for each grid cell in the height map, the maximum height (Z coordinate) of the workpiece 300 in the reference coordinate system is determined.

[0021] The height map image 320 depicts the workpiece 300 from an "upward" perspective, i.e., perpendicular to the XY plane of the height map reference coordinate system. The colors or grayscale shading in the image 320 represent the height values ​​of each grid cell in the height map. The height map image 320 is provided simply as a visual representation of the height map concept; the image 320 itself is not used in subsequent collision-checking calculations. The height map 330 is a two-dimensional (2D) matrix with a number of rows and columns determined by the size of the workpiece 300 and the X and Y grid cell sizes. Each grid cell in the height map 330 contains the maximum height of the workpiece 300 within that grid cell. That is, the height map 330 defines z(x,y), where z is the height value (Z coordinate) in the height map reference coordinate system for a given grid cell (x,y pair). The height map 330 is used for collision checking of toolpaths, as described above and in more detail below.

[0022] Height map 330 shows numbers in some matrix cells and ellipsis ("...") filled in the rest of the matrix, and is simplified in Figure 3. Height map 330 is meant to illustrate the concept; the numbers shown in some matrix cells are not meant to be significant or easily discernible.

[0023] 4 is an illustrative flow diagram of a method for using the height map of FIG. 3 to perform a collision check on a tool center point path and define a new collision-free path based on the detected collision using the height map, according to an embodiment of the present disclosure. A CAD model of a workpiece 400 is provided as input in the same manner as described above. Path points for the tool path are also provided as input in box 410. The path points for the tool path provided in box 410 include at least a start point and an end point, and may also include intermediate path points that are connected by piecewise straight line segments, spline curves, or some other path shape to define the tool trajectory.

[0024] A height map 420 is calculated for the workpiece 400 in the manner described above. The height map 420 is shown in FIG. 4 as a visual representation, and it should be understood that a height map numerical matrix representing z(x,y) for the workpiece 400 is calculated and used in the collision avoidance calculations. A tool center point path 430 is shown superimposed on the height map 420. The tool center point path 430 in this case is a straight line from a start point 432 to an end point 434. This may be the case, for example, if a tool is machining a hole at one corner of the workpiece 400, the tool needs to rise and traverse along the path 430 before machining a hole at another corner of the workpiece 400. The tool center point path 430 needs to be translated into the height map reference coordinate system in order to perform interference checking calculations using the height map.

[0025] At box 440, the tool center point path 430 is shown in an isometric view along with the workpiece 400, where it can be seen that the tool center point path 430 passes over and through the workpiece 400. The tool center point path 430 can be identified at box 440 as interfering with two different portions of the workpiece 400. At box 450, the workpiece 400 is shown with the tool center point path 430 along with a collision avoidance path 460 (dashed line). Using the height map 420 in the calculations described below, it is determined that the tool center point path 430 interferes with the workpiece 400 at two locations, indicated by points 472 and 474. At point 472, a new collision avoidance path point 462 is defined, and at point 474, a new collision avoidance path point 464 is defined. The new collision avoidance path points 462 and 464 are defined with a vertical offset from the collision check points that are part of the tool center point path 430 and that were determined to interfere with the workpiece 400. A collision-free path 460 is calculated from the original start point 432 to the end point 434 using new collision-free path points 462 and 464 as intermediate path points. A height map interference check calculation may be performed again on the collision-free path 460 to ensure no interference with the workpiece 400 remains.

[0026] The collision check calculation of the tool center point path 430 against the height map 420 is a very fast calculation, as is the calculation of the new collision-free path points, which is described below.

[0027] 5 is a flowchart 500 of a method for performing a collision check on a tool center point path and determining a new collision-free path based on the detected collision using a height map, according to an embodiment of the present disclosure. The flowchart in FIG. 5 is arranged into steps performed by a collision checker at block 510 and a motion planner at block 520.

[0028] The input to the collision checker block 510 includes object models of the workpiece and any other objects that may be obstacles to the tool path. In the example described above, the obstacle was the workpiece being manipulated by the machine tool. Other workspace obstacles may include, for example, fixed objects. The object models are typically provided as CAD solid or surface models. The input to the motion planner block 520 is an initial set of command path points. The command path points are typically for an air cutting process as described above, e.g., moving the tool from one hole machining location to another hole machining location.

[0029] In box 512, a height map for the obstacle (e.g., workpiece) is calculated and loaded. This involves placing the workpiece object model into a height map reference coordinate system (such as the workcell "world" coordinate system, or a coordinate system with the XY plane of the reference coordinate system at the table top in the workcell). With the workpiece object model placed into the height map reference coordinate system, the height map is calculated as described above, providing a 2D matrix of height values ​​relative to the XY grid of the cell.

[0030] In box 522, a toolpath is generated for the commanded path points provided as input. In the example described above, the initial toolpath was a straight line from the start point to the end point. However, this was just one example. The toolpath may have other shapes, such as, for example, a spline function fitted through several commanded path points. In box 524, the toolpath (trajectory) is discretized into a set of collision checkpoints. Collision checkpoints are individual points along the continuous toolpath that are compared to the height map.

[0031] At box 514, each of the collision check points from box 524 is collision checked against the height map from box 512. This calculation is described below with respect to FIG. 6. At decision diamond 516, it is determined whether any collisions were detected at box 514. If any collisions are detected, at box 518, collision-free path points are generated for any collision check points that collided with the height map. The generation of these collision-free path points was shown in box 450 of FIG. 4. After the new collision-free path points are generated at box 518, the process returns to box 522 to generate a new toolpath using the collision-free path points. The new collision-free path points are used in addition to the original commanded path points to generate the new toolpath. The new toolpath is then discretized at box 524, and the height map collision checking process is repeated.

[0032] When the collision checking process does not result in a collision, the process moves from decision diamond 516 to block 530 where new commanded path points are output and used for motion control of the machine tool. The new commanded path points include a combination of the original commanded path points (typically at least the start and end points) and any collision avoidance path points generated in box 518 earlier in the process.

[0033] FIG. 6 includes a diagram 600 of a technique for interference checking individual path points relative to a machine tool center point path using an obstacle height map, and a flowchart diagram 650 of corresponding method steps, according to an embodiment of the present disclosure. The height map 610, as described above, includes height values ​​for a 2D matrix of cells in an XY grid. The height map 610 represents any obstacle environment that may be present in the work cell, e.g., a workpiece, a workpiece with fixtures, etc. The height map 610 is shown somewhat conceptually in FIG. 6, with most height value numbers replaced with ellipses for simplicity. The grid cells of the height map 610 are shaded to represent various ranges of height values, as previously described.

[0034] A number of collision check points (620, 622, and others not numbered) are shown superimposed on the height map 610. The collision check points 620 / 622 / etc. are discretized representations of the tool center point path, as described above with respect to Figure 5. Flowchart diagram 650 defines the steps performed in collision check box 514 of Figure 5.

[0035] In box 652, a collision checkpoint (e.g., checkpoint 620) is provided for the toolpath. The checkpoint is a location in the coordinates of the heightmap reference coordinate system (x tcp ,y tcp , z tcp ) that are outside the height map region. At decision diamond 654, it is determined whether the checkpoint coordinates are within the region of the height map 610. For example, based on the size of the workpiece, the height map 610 may have a size in the height map reference coordinate system of 500 mm x 500 mm, where the height map is defined for the coordinate range 0≦x≦500 and 0≦y≦500. At decision diamond 654, it is determined whether the checkpoint has X and Y coordinates (x tcp ,y tcp ), the process returns to box 652 to provide the next checkpoint.

[0036] If the checkpoint is within the heightmap region, then in box 656, enter the checkpoint XY coordinates (x tcp ,y tcp ) is converted to a height map row-column index. For example, if height map 610 defines a 500 mm by 500 mm space and the grid cell size is a 5 mm square, then height map 610 has 100 rows and 100 columns of grid cells. The calculation in box 656 is tcp ,y tcp ) the row and column of the grid cell occupied by the checkpoint with X and Y coordinates of tcp ,c tcp ) to identify the

[0037] At decision diamond 658, the collision check calculation is performed using the Z coordinate of the check point (z tcp) minus the margin of safety (d safe ) is the height map 610 value (Hmap(r tcp ,c tcp )). If the answer is yes at decision diamond 658, this means there is a collision, and the process returns a collision status at box 660. This corresponds to yes at decision diamond 516 in FIG. 5. If the answer is no at decision diamond 658, this means the checkpoint being evaluated is safely above the height map, and the process returns to box 652 to evaluate the next checkpoint. This process continues until no checkpoints remain in the toolpath.

[0038] The height map collision checking technique described above provides very fast and accurate collision checking results for obstacles in the machine tool workspace. However, in some applications, collision checking of the tool center point alone is not sufficient to ensure a collision-free machine tool motion plan. An enhanced technique to address this situation is described below.

[0039] 7 is a diagram of a machine tool operating in an environment of workpiece obstacles, depicting how a portion of the tool body or holder may collide with the obstacle if only tool center point collision checking is performed, according to an embodiment of the present disclosure. Tool body 710 holds a cutting tool 720. Tool body 710 includes a tool holder connected to the machine tool spindle. Thus, tool body 710 is larger than the cutting tool 720 itself. Cutting tool 720 is performing an action relative to a workpiece 730 in the same manner as previously described.

[0040] Cutting tool 720 has a tool center point 722. Using the tool center point for the height map collision checking technique described above, a collision-free toolpath 740 is calculated that includes path points including a start point (at the location indicated for the tool center point 722), two collision-free path points above the highest edge of the workpiece 730, and an end point 742. However, due to the combination of tool body shape, workpiece shape, and toolpath geometry, it is possible that point 712 on tool body 710 may collide with point 732 on workpiece 730 when tool center point 722 follows collision-free toolpath 740. To address this situation and ensure collision-free tool operation, the tool needs to be modeled as more than just a tool center point.

[0041] 8 is an illustration of a machine tool and corresponding tool height map used in a map-to-map interference checking technique according to an embodiment of the present disclosure. Tool 810 includes the cutting tool and any components connecting the cutting tool to the machine spindle that may be of concern for interference with the workpiece or other obstacles, which may generally be considered the tool body along with the cutting tool itself.

[0042] A tool height map point set 820 is created for the tool 810. The tool height map point set 820 is a "bottom-up" view of the tool 810, including the cutting tool itself along with the tool body. In a preferred embodiment, the tool height map point set 820 places the tool center point at coordinates (0,0,0) in the local tool coordinate system, and the points in the tool height map point set 820 all have positive Z coordinates. When visually comparing the tool height map point set 820 to the tool 810, it can be seen that the tool height map point set 820 includes a small cluster of points at the tool center point, a ring of points with a concave upward curvature corresponding to the lower portion of the tool body, and two more rings of points corresponding to shoulders of increasing diameter on the tool body.

[0043] A tool height map 830 is created from the tool height map point set 820. The tool height map 830 is shown as a visual representation, where different shading intensities represent different heights. Viewed from bottom to top, any "pixel" within the XY grid of the tool height map 830 has a height value based on the Z coordinate of the point within that pixel. It should be understood that, as previously described with respect to the other height maps, a 2D numerical matrix or grid of pixels is created for the tool height map 830, including data defining z(x,y). This grid of height data is what is used in the calculations described below.

[0044] The tool height map 830 is based on the actual shape of the tool body. In some embodiments, it may be desirable to provide a clearance margin around the outside of the tool body when checking for tool-to-work interference. Techniques for including a radial offset margin in the tool height map are described below.

[0045] 9 is an illustrative flow diagram of a method for using an obstacle height map and a tool height map to perform collision checking of a machine tool path and to define a new collision-free path based on the detected collision using the height map, according to an embodiment of the present disclosure. A CAD model of a workpiece 900 is provided as input, and path points for the tool path are also provided as input in box 910, in the same manner as described above. A CAD model of a tool 920 is also provided. The tool 920 includes the tool body as well as the cutting tool itself, as described above.

[0046] An initial toolpath is generated in box 930 from the command path points in the same manner as described above. An obstacle height map 940 is calculated for the workpiece 900. This involves defining a height map reference coordinate system, defining a grid of cells in the XY plane of the reference coordinate system, and calculating the maximum height of the workpiece in the Z direction for each grid cell. The obstacle height map 940 is a "top-down" view of the workpiece 900 (and any associated obstacles, such as fixtures), where the maximum value in the obstacle height map 940 corresponds to the highest point on the workpiece 900.

[0047] A tool height map 950 is then calculated for the tool 920. The tool height map 950 is a "bottom-up" view of the tool 920, where each "pixel" (grid cell) in the tool height map 950 has a height value that specifies the Z offset of that point on the tool 920 above the tool center point (the lowest point on the tool 920). The Z offset values ​​in the tool height map 950 are measured in the same Z direction as the obstacle height map reference coordinate system. In a preferred embodiment, the tool center point has a tool height map value of zero, and all other points (pixels) on the tool body have positive tool height map values. Other reference coordinate system conventions may also be used.

[0048] Both the obstacle height map 940 and the tool height map 950 are shown in pictorial form for illustrative purposes, where different shading intensities represent different heights. The numerical data defining z(x,y) is of course used in the collision checking calculations described below.

[0049] At box 960, the obstacle height map 940 is collision-checked against the toolpath in a similar manner as described above. However, in this case, rather than simply checking the tool center point against the obstacle height map 940 for each checkpoint, multiple pixels of the tool height map 950 are checked against the obstacle height map 940 for each checkpoint location. For example, for the first checkpoint, the tool height map 950 is positioned as shown at 950A, where each pixel of the tool height map 950 is checked against a corresponding grid cell of the obstacle height map 940. After all of the pixels of the tool height map 950 have been checked for the first toolpath checkpoint, the tool height map 950 is moved to the location of the next checkpoint, as shown at 950B. This process is repeated for the entire toolpath from box 930. For any detected collision between the tool height map 950 and the obstacle height map 940, a new collision-free path point is generated. The new collision-free path point is generated in any suitable manner, for example, by determining the amount of interference between the tool height map pixel and the obstacle height map grid cell and adding a vertical offset of that amount (plus an extra amount, if necessary) to the tool path checkpoint to obtain the new collision-free path point.

[0050] At box 970, a collision-free toolpath 980 is generated using the new collision-free path points. The collision-free toolpath 980 avoids interference between any part of the tool 920 and any part of the workpiece 900 by collision checking the tool height map 950 against the obstacle height map 940. The workpiece 900 and obstacle height map 940 may, of course, include other obstacles, such as fixtures, as previously described.

[0051] FIG. 10 includes a diagram 1000 of a technique for collision checking path points of a machine tool path using an obstacle height map and a tool height map, in which each pixel in the tool height map is collision checked for each path point, according to an embodiment of the present disclosure, as well as a flowchart diagram 1050 of corresponding method steps. The obstacle height map 1010 includes height values ​​for a 2D matrix of cells in an XY grid, as described above. The height map 1010 corresponds to the height map 940 graphically depicted in FIG. 9. The height map 1010 represents any obstacle environment that may be present in a work cell, e.g., a workpiece, a workpiece plus fixtures, etc. The height map 1010 is again shown somewhat conceptually in FIG. 10, with most height value numbers replaced with ellipses for simplicity. The grid cells of the height map 1010 are shaded to represent various ranges of height values, as previously described.

[0052] The tool height map is shown superimposed on the height map 1010 at multiple collision check points (1020, 1022, and others not numbered). The tool height map shown in diagram 1000 corresponds to tool height map 950 of FIG. 9. Collision check points 1020 / 1022 / etc. are discretized representations of the tool center point path, as described above. Flowchart diagram 1050 defines the steps taken to collision check the tool height map against an obstacle (e.g., workpiece) height map.

[0053] In box 1052, a collision checkpoint is provided for the toolpath. The checkpoint is a position in the coordinates of the reference coordinate system of the obstacle height map (x tcp ,y tcp ,z tcp ) in box 1054. The tool height map is positioned so that the location of the tool center point in the tool height map is at the current check point of the tool path. For example, in diagram 1000, the tool height map is first moved to check point 1020. In box 1056, an individual pixel i is selected in the tool height map. Pixel i has coordinates (x i ,yi ,z i ) Then, the collision check for pixel i is performed in the following steps:

[0054] At decision diamond 1058, it is determined whether the X and Y coordinates of pixel i for the current checkpoint are within the region of the obstacle height map 1010. Pixel i has X and Y coordinates (in the obstacle height map reference coordinate system) that are found by taking the X and Y coordinates of the current toolpath checkpoint and adding the X and Y coordinates of pixel i relative to the tool center point in the tool height map. At decision diamond 1058, if checkpoint i has X and Y coordinates that are outside the obstacle height map region, the process returns to decision diamond 1060.

[0055] If the coordinates of pixel i for the current checkpoint are within the obstacle height map region, then in box 1062 the XY coordinates of pixel i for the current checkpoint are converted to obstacle height map row-column indices. In a manner similar to that described above with respect to FIG. 6, the calculation in box 1062 is i ,y i ) the row and column (r i , c i ) to identify the

[0056] At decision diamond 1064, the collision check calculation begins with the Z coordinate of pixel i relative to the current check point (z i ) minus the margin of safety (d safe ) is the value of the obstacle height map 1010 (Hmap(r i ,c i )) The Z coordinate of pixel i is determined by taking the Z coordinate of the current toolpath checkpoint and adding the Z coordinate of pixel i to the tool center point in the tool height map.

[0057] If the answer is yes at decision diamond 1064, the process returns a collision status at box 1066. If the answer is no at decision diamond 1064, which means that pixel i of the tool height map for the checkpoint being evaluated is safely above the obstacle height map 1010, the process returns to decision diamond 1060. If, at decision diamond 1060, more pixels of the tool height map are still to be evaluated for the current toolpath checkpoint, the process returns to box 1056, where the value of i is incremented by one (i=i+1), and the new pixel i is evaluated using the steps below box 1056 as described above. This process continues until all (e.g., several hundred) of the pixels of the tool height map have been evaluated for the current toolpath checkpoint. The spatial resolution and resulting number of pixels in the tool height map may be selected as appropriate to provide the desired degree of accuracy for a particular application.

[0058] If, at decision diamond 1060, there are no more tool height map pixels left to evaluate for the current toolpath checkpoint, the process returns to box 1052 to evaluate the next checkpoint in the toolpath. This process continues until no more checkpoints remain in the toolpath.

[0059] The collision indicated in box 1066 may be addressed by immediately calculating a new collision-free path point that has a higher height than the current checkpoint, as previously described. Alternatively, all of the collisions indicated in box 1066 may be stored until all pixels of the tool height map have been collision-checked for all of the toolpath checkpoints, and then each checkpoint with any tool height map pixel collisions may generate a new collision-free path point based on all of the tool height map pixel collision data for that checkpoint.

[0060] To summarize the foregoing, the present disclosure, which is particularly applicable to machine tool applications, enables rapid collision avoidance calculations in motion planning using height maps. A height map of the workpiece and / or any other obstacles in the workcell is created, and a representation of the machine tool is collision checked against the obstacle height map at points along the tool path. The representation of the machine tool may simply be the tool center point (i.e., the tip of the cutting tool), or the representation may be a tool height map in the form of a pixel map of heights.

[0061] The above-described techniques enable extremely fast collision checking calculations to be performed in real time during motion planning, particularly when the machine tool is represented as only the tool center point. However, when a height map is used to represent the machine tool, the number of calculations required increases significantly and the calculation speed deteriorates accordingly. The following description is directed to a technique for improving the speed of real-time collision checking calculations by pre-generating a workspace collision map and then performing fast point-to-map collision checking calculations during runtime motion planning.

[0062] 11 is a flowchart diagram 1100 of a method for performing interference checking between a tool and workspace obstacles, including pre-generation of a workspace collision map and rapid tool path interference checking using the collision map, according to an embodiment of the present disclosure. The flowchart diagram of FIG. 11 is arranged with the collision map pre-generation process above the dashed line and the runtime motion planning process below the dashed line.

[0063] Map generator block 1110 includes a tool map reconstruction box 1120 and a workspace map scanning box 1130. The steps performed in tool map reconstruction box 1120 are shown in Figure 12, and the steps performed in workspace map scanning box 1130 are shown in Figure 13.

[0064] 12 is an illustration of a technique for reconstructing a tool height map with a radial margin, according to an embodiment of the present disclosure. An original tool height map 1210 and its corresponding original tool height map point set 1220 are shown in the upper left. The original tool height map 1210 and original tool height map point set 1220 are based on the actual shape of the tool body, as previously described with respect to FIG. 8. The following steps are used to construct a new tool height map that incorporates a radial clearance margin around the tool body:

[0065] Box 1230 contains a plan view of the original points in an inner circle 1240 (the points are from the original tool height map point set 1220) and the clearance points in an outer ring 1242. For each of the original points, a clearance point is created that incorporates a radial clearance margin around the tool body. For example, for original point 1250, a radial offset vector 1252 is applied, resulting in clearance point 1254. The radial offset vector 1252 is calculated relative to the tool height map center point 1256, which has coordinates (0,0,0), as follows:

[0066] Consider an original point 1250 with tool height map coordinates (x,y,z). The coordinates of the clearance point 1254 are calculated using the following formula:

number

number

number

number

[0067] The combined point set 1260 is shown in the upper right of Figure 12 in an isometric view, where the points from the original tool height map point set 1220 are visible and the margin points are clearly visible in the outer bands 1262, 1264, and 1266. A new tool height map 1270 is shown in graphical form including data from the original tool height map 1210 described above and data from the radial margin points. It should be understood that, as previously described with respect to the other height maps, a 2D numerical matrix or grid of pixels is created for the new tool height map 1270, including data defining z(x,y). This grid of height data is what is used in the calculations described below.

[0068] 13 is a diagram of a technique for creating a workspace collision map by scanning a tool height map over an obstacle height map, according to an embodiment of the present disclosure. An obstacle height map 1310 is provided, as mentioned several times before. A tool height map 1320 is also provided. In FIG. 13, the tool height map 1320 is shown as being the new tool height map 1270 of FIG. 12, incorporating a radial offset margin. However, the original tool height map 1210 (without the radial margin) could be used instead. Whether to include a radial margin around the tool height map, and if so, how much, is a matter of preference for any given application.

[0069] The concept of the workpiece collision map is realized through the following steps. Scan the tool height map over the obstacle height map to create a collision map grid, where each grid cell corresponds to a scan point. For each scan point, determine what height the tool height map needs to have to avoid any collision between any pixel in the tool height map and any cell in the obstacle height map. Each grid cell of the collision map is then populated with the height value calculated when the tool height map places the tool center point at that grid cell of the collision map.

[0070] In the lower left of Figure 13, scan path 1330 is shown as having a back and forth shape. This is for illustrative purposes only. The actual scan pattern used, and the size of the collision map grid, is a matter of design preference. The collision map grid needs to have sufficient resolution so that when the tool center point is later collision checked against the collision map, the results are accurate. For example, the collision map could represent the workspace on a 100 cell by 100 cell grid.

[0071] Box 1340 includes a diagram of tool height map 1320 at a scan point on obstacle height map 1310 and is used to explain the scanning process and calculations. Scan point 1350 is a point along scan path 1330. Around scan point 1350, patch 1360 is drawn in obstacle height map 1310. Patch 1360 has a size that matches the size of the rectangle drawn around tool height map 1320. Sub-height map 1370 is created by cropping obstacle height map 1310 to the size of patch 1360. Sub-height map 1370 and tool height map 1320 each have the same size dimensions in the X and Y directions and the same pixel resolution (e.g., 100 pixels by 100 pixels). Therefore, there is a one-to-one correspondence between pixels in sub-height map 1370 and pixels in tool height map 1320.

[0072] For scan point 1350, a collision map height value h is calculated using the sub-height map 1370 and the tool height map 1320 as follows: The collision map height value h is the height that the tool height map 1320 needs to be raised above the sub-height map 1370 to avoid any interference. A procedure is used in which the value of h is set to zero, the calculation is made, and then h is increased to a value that does not result in interference between the sub-height map 1370 and the tool height map 1320.

[0073] A temporary matrix tmp is defined with dimensions equal to the number of pixels in the sub-height map 1370 and the tool height map 1320 (e.g., 100 pixels by 100 pixels, using the same example as above). The values ​​in matrix tmp are calculated by adding h (initially zero) to the value of each pixel in the tool height map 1320 (toolHmap+h), and then subtracting the value of each pixel in the sub-height map 1370 plus the vertical gap margin (subHmap+marginZ).

[0074] Therefore, the matrix tmp is calculated as follows: tmp=(toolHmap+h)-(subHmap+marginZ) (5) where toolHmap is a matrix of tool height map pixels and subHmap is a matrix of sub-height map pixels with 1:1 pixel correspondence.

[0075] After calculating the matrix tmp for the current scan point and the current value of h, it is determined whether any values ​​in the matrix tmp are less than zero. If so, this means that there is interference between at least one of the pixels of the sub-height map 1370 and the corresponding pixel of the tool height map 1320. In this case, the value of h is increased by the amount of interference (to offset the most negative value in tmp), and the matrix tmp is recalculated for the same current scan point. Values ​​of h that do not result in a negative value in the matrix tmp represent the tool center point height required to avoid any interference between the tool height map 1320 and the sub-height map 1370 at the current scan point.

[0076] The above calculations result in a height value h for scan point 1350, and this value of h is used in the collision map grid cell that corresponds to scan point 1350. This process is repeated for all of the scan points along scan path 1330, resulting in a grid of height values ​​that covers the entire workpiece.

[0077] 3D model 1380 depicts the results of the collision map calculations described above. It can be seen that 3D model 1380 generally appears similar to the 3D models of the workpiece shown previously (in FIGS. 7 and 9). However, compared to the 3D model of the workpiece, 3D model 1380 includes raised shoulders and platforms that are the result of lateral and diagonal interference between the tool and the workpiece. Such lateral and diagonal interference is detected using the collision map construction process described above, which scans the tool height map over the obstacle height map. Collision map 1390, shown in 2D graphical form, is the final result of this process. It should be understood that, as described above with respect to the height map, a 2D numerical matrix or grid of pixels is created for collision map 1390, including data defining z(x,y). The tool center point path can be evaluated against this grid of height data to quickly perform trajectory interference checks that incorporate lateral and diagonal interference detection as described above.

[0078] Returning to Figure 11, the collision map 1390 calculated as shown in Figure 13 is used as the collision map 1140. The collision map 1140 is provided as an input to the collision checker block 1150, and the commanded path points are provided as input to the motion planner block 1160. The process steps performed by the collision checker block 1150 and the motion planner block 1160 are essentially the same as the similar elements of Figure 5 described above, except that in the case of Figure 11, the tool center point path is checked against the collision map 1140, which incorporates both obstacle height map and tool height map data.

[0079] In box 1162, a trajectory is planned based on the commanded path points provided as input. This involves both generating the tool path and discretizing the tool path into a set of collision check points. Collision check points are individual points along the continuous tool path that are compared to a collision map. These steps were described above with respect to FIG. 5.

[0080] At box 1152, each of the collision check points from box 1162 is collision checked against the collision map 1140. This calculation was described above with respect to FIG. 6, where each path check point is mapped to a grid cell of the collision map 1140 and the height (Z coordinate) of the path check point is compared to the height value of the grid cell. At decision diamond 1154, it is determined whether any collisions were detected at box 1152. If any collisions are detected, at box 1156, collision-free path points are generated for any collision check points that collided with the collision map 1140. After new collision-free path points are generated at box 1156, the process returns to box 1162 to generate a new tool path using the collision-free path points. The new tool path trajectory is then discretized and the collision-map interference checking process is repeated.

[0081] When the collision checking process does not result in a collision, the process moves from decision diamond 1154 to block 1170 where new commanded path points are output and used for motion control of the machine tool. The new commanded path points include a combination of the original commanded path points (typically at least the start and end points) and any collision avoidance path points generated in box 1156 in the previous step through the collision map checking process.

[0082] Recall that in FIG. 11 , the collision map generation calculations (above the dashed line) are performed in one initial step for a given machining workspace (combination of tool, workpiece, and other obstacles). This collision map generation is a lengthy computational process; in experimental implementations of the disclosed techniques, collision map generation took over 1.5 seconds to complete on a given computing device. However, once the collision map is generated, collision check calculations for the tool path trajectory are very fast, on the order of a few milliseconds for the same computing device. This is because trajectory collision checking only involves point-pair map calculations for the tool center point check points, and the tool height map data is already incorporated into the collision map. The collision check calculations are fast enough to be performed in real time during motion planning using a machine controller as the computing device.

[0083] As previously described, interference checking using the collision map technique of Figures 11-13 is performed before the machine tool is physically moved along the trajectory. When interference is detected in a future motion step using the collision map interference checking method, the machine controller calculates collision-free path points and calculates a new collision-free trajectory before moving the tool.

[0084] Again, all of the method steps shown in Figures 11-13 are programmed in an algorithm executed on a computing device having a processor and memory. The machine controller preferably performs the motion planning and collision checking steps of Figure 11, while collision map generation may occur in the machine controller or a separate computing device.

[0085] As outlined above, the disclosed techniques for machine tool collision avoidance using height maps and collision maps improve the speed and accuracy of interference checking compared to prior art methods. Evaluation of the disclosed height map and collision map interference checking method shows that the technique is accurate, can accommodate lateral and diagonal interference between the tool and workpiece when needed, and is much more computationally efficient than existing techniques such as axis-aligned bounding box tree methods.

[0086] While numerous preferred aspects and embodiments of the object interference checking technique using height maps and collision maps have been described above, those skilled in the art will recognize modifications, permutations, additions, and subcombinations thereof. Accordingly, it is intended that the following appended claims and the claims incorporated below be interpreted as including all such modifications, permutations, additions, and subcombinations as are within the true spirit and scope thereof.

Claims

1. 1. A method for collision checking a path of a moving object in a workspace, the method being performed in a computing device having a processor and a memory, the method comprising: creating a first height map having an array of pixels each defining a height of an obstacle in the workspace above a reference surface; creating a second height map having an array of pixels each defining a height of a downward-facing surface of the object above a reference point on the object; generating a collision map comprising defining a grid of cells in the reference surface and determining, for each grid cell, the height of the reference point of the object required to avoid interference between the second height map and the first height map; determining a plurality of check points for the object along the path; performing an interference check for each of the check points by comparing the height of the reference point above the reference surface with the collision map.

2. The method of claim 1 , wherein the workspace obstructions include one or more of a workpiece, a fixture, a table, or other object on which a tool performs an operation.

3. The method of claim 1 , wherein the reference plane is a major surface of a reference coordinate system having a defined position and orientation relative to a world coordinate system of a workspace, including any combination of relative positions and orientations.

4. The method of claim 1 , wherein the collision map is generated once and the interference check is performed for multiple paths.

5. 2. The method of claim 1, wherein the object is a tool, the path is a tool path, the first height map is a workspace height map, the second height map is a tool height map, and the reference point is a tool center point.

6. 6. The method of claim 5, wherein creating the tool height map includes adding a radial offset margin to each point on the downward facing surface of the tool used to create the tool height map.

7. The method of claim 5 , wherein the heights in the workspace height map, the heights in the tool height map, and the heights in the collision map are all measured in a vector direction normal to the reference plane.

8. 6. The method of claim 5, wherein generating a collision map comprises: for each grid cell of the collision map, locating the tool height map relative to the tool center point at a position on the grid cell; defining a workspace height sub-map from a projection of the tool height map onto the workspace height map; and determining, for each pixel of the tool height map, the height required to avoid collision with a corresponding pixel of the workspace height sub-map.

9. 9. The method of claim 8, wherein locating the tool height map relative to the tool center point at a location on the grid cell, defining a workspace height sub-map from a projection of the tool height map onto the workspace height map, and identifying corresponding pixels in the workspace height sub-map are all performed using a vector direction normal to the reference plane.

10. The method of claim 8 , wherein for each grid cell of the collision map, a height value is input that is equal to the maximum of the determined heights for all of the pixels of the tool height map.

11. The method of claim 8 , wherein determining the height required to avoid collision with a corresponding pixel of the workspace height sub-map includes incorporating a vertical clearance margin.

12. The method of claim 5 , further comprising: calculating collision-free path points for any of the check points that result in a collision in the interference check; and calculating a new tool path using the collision-free path points.

13. The method of claim 12 , further comprising transmitting, by the computing device, motion commands to a machine tool to cause the machine tool to move the tool according to the new tool path.

14. 1. A method for interference checking a path of a tool moving in a workspace, the method being performed on a computing device having a processor and a memory, the method comprising: creating a workspace height map having an array of pixels each defining a height of an obstacle in the workspace above a reference surface; creating a tool height map having an array of pixels each defining a height of a downward facing surface of the tool above a tool center point; generating a collision map comprising: defining a grid of cells in the reference plane; and determining for each grid cell the height of the tool centre point required to avoid collisions between the tool height map and the workspace height map, the collision map comprising locating the tool height map relative to the tool centre point at a position on the grid cell; defining a workspace height sub-map from a projection of the tool height map onto the workspace height map; and determining for each pixel of the tool height map the height required to avoid collisions with a corresponding pixel of the workspace height sub-map; determining a plurality of check points of the tool center point along the path of the tool; performing an interference check for each of the check points by comparing the height of the tool center point above the reference plane with the collision map; calculating a collision-free path point for any of the check points that cause a collision in the interference check, and calculating a new tool path using the collision-free path point; transmitting, by the computing device, motion commands to a machine tool to cause the machine tool to move the tool according to the new tool path.

15. a machine for moving a tool in a work space; a computing device having a processor and a memory; The computing device is in communication with the machine, creating a workspace height map having an array of pixels each defining a height of an obstacle in the workspace above a reference surface; creating a tool height map having an array of pixels each defining a height of a downward facing surface of the tool above a tool center point; generating a collision map comprising defining a grid of cells in the reference plane and determining, for each grid cell, the height of the tool center point required to avoid interference between the tool height map and the workspace height map; determining a plurality of check points for the tool along the tool path; performing an interference check for each of the check points by comparing the height of the tool center point above the reference plane with the collision map.

16. The system of claim 15 , wherein the workspace obstructions include one or more of a workpiece, a fixture, a table, or other object on which a tool performs an operation.

17. The system of claim 15 , wherein the reference plane is a major surface of a reference coordinate system having a defined position and orientation relative to a world coordinate system of a workspace, including any combination of relative positions and orientations.

18. The system of claim 15 , wherein the collision map is generated once and the interference check is performed for multiple paths.

19. 16. The system of claim 15, wherein creating the tool height map includes adding a radial offset margin to each point on the downward facing surface of the tool used to create the tool height map.

20. The system of claim 15 , wherein the heights in the workspace height map, the heights in the tool height map, and the heights in the collision map are all measured in a vector direction normal to the reference plane.

21. 16. The system of claim 15, wherein generating a collision map comprises: for each grid cell of the collision map, locating the tool height map relative to the tool center point at a position on the grid cell; defining a workspace height sub-map from a projection of the tool height map onto the workspace height map; and determining, for each pixel of the tool height map, the height required to avoid collision with a corresponding pixel of the workspace height sub-map.

22. 22. The system of claim 21, wherein locating the tool height map relative to the tool center point at a location on the grid cell, defining a workspace height sub-map from a projection of the tool height map onto the workspace height map, and identifying corresponding pixels in the workspace height sub-map are all performed using a vector direction normal to the reference plane.

23. 22. The system of claim 21, wherein for each grid cell of the collision map, a height value is input that is equal to the maximum of the determined heights for all of the pixels of the tool height map.

24. 22. The system of claim 21, wherein determining the height required to avoid collision with a corresponding pixel of the workspace height submap includes incorporating a vertical clearance margin.

25. 16. The system of claim 15, wherein the computing device is further configured to calculate collision-free path points for any of the check points that result in a collision in the interference check, and to calculate a new tool path using the collision-free path points.

26. 26. The system of claim 25, wherein the computing device is further configured to send motion commands to the machine to cause the machine to move the tool according to the new tool path.

Images