Collision-avoidance motion planning method for industrial robots

Abstract

A robot collision avoidance motion planning technique that uses worst-case search and optimization. The motion planning technique begins with a geometric definition of obstacles, start and goal points, and an initial set of waypoints that may be sparsely spaced. Given a point-to-point interpolation method such as linear or spline, a continuous trajectory can be described as a function of waypoint and arc-length parameters. A worst-case search is then performed to find the location between each adjacent pair of waypoints that has the worst-case condition for distance to obstacles by considering all parts of the robot and tool. Collision avoidance constraints are defined using the worst-case locations, and optimization of the waypoint locations is performed to improve the worst-case condition until all collisions are eliminated and the obstacle avoidance minimum distance criterion is satisfied.

Description

[Technical field]

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS This application claims the benefit of the priority date of U.S. Provisional Patent Application No. 62 / 978,654, filed February 19, 2020, and entitled “COLLISION AVOIDANCE MOTION PLANNING METHOD FOR INDUSTRIAL ROBOT.”

[0002] The present disclosure relates to the field of motion control of industrial robots, and more particularly to a robot collision avoidance motion planning technique that begins with the definition of an initial set of obstacles and sparsely spaced waypoints, finds locations between each adjacent pair of waypoints that have the worst case scenario of distance to the obstacles, and then performs an optimization of the waypoint locations to improve the worst case scenario up to the point where a minimum obstacle avoidance distance criterion is satisfied. [Background technology]

[0003] The use of industrial robots to perform various manufacturing, assembly, and material transfer operations is well known. In many robot workspace environments, obstacles are present that may be located in the path of motion of the robot. The obstacles may be permanent structures such as machines and fixtures. They may also be containers in which a workpiece or part being manipulated is being placed, in which the robot must manipulate the workpiece or container in and out of while performing the operation. Collisions between the robot and any obstacles must be avoided at all costs.

[0004] Prior art techniques for robot collision avoidance motion planning typically involve defining a set of waypoints along a trajectory and checking for collisions or excessively small clearances at the distinct states represented by each of the waypoints. In these techniques, it is possible that collisions between thin obstacles may be missed in the motion plan if the thin obstacles happen to be present between the predefined waypoints. The only sure way to ensure that thin obstacles are not missed is to define a dense set of waypoints, where the spacing between the waypoints is not large enough to allow for occupation by the thin obstacles.

[0005] And the use of dense waypoint spacing, as required by prior art techniques, creates other problems. One problem with dense waypoint spacing is that motion planning computation time increases because inverse kinematic robot configuration and collision avoidance optimization calculations must be performed for each waypoint. Another problem with dense waypoint spacing is that curvature blending deceleration is often required near each waypoint, often creating uneven or jerky robot motion. Summary of the Invention [Problem to be solved by the invention]

[0006] In view of the above, a need exists for improved robot motion optimization techniques that do not require dense waypoint spacing, yet still reliably identify and automatically resolve any collisions or minimum distance threshold violations along the robot's trajectory. [Means for solving the problem]

[0007] According to the teachings of the present disclosure, a robot collision avoidance motion planning technique using worst-case search and optimization is disclosed. The motion planning technique begins with a geometric definition of an obstacle, a start and finish point, and an initial set of waypoints that may be sparsely spaced. Given a point-to-point interpolation method such as linear or spline, a continuous trajectory can be described as a function of the waypoints and arc-length parameters. By considering all parts of the robot and tool, a worst-case search is performed to find the location between each adjacent pair of waypoints that has the worst case of distance to the obstacle. Using the worst-case locations, a collision avoidance constraint is defined, and an optimization of the waypoint locations is performed to improve the worst case until all collisions are eliminated and the obstacle avoidance minimum distance criterion is satisfied.

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

[0009] [Figure 1] FIG. 1 is an illustration of an industrial robot operating in the vicinity of an obstacle, where traditional collision avoidance motion planning techniques may fail to identify the obstacle collision if sufficiently dense waypoint spacing is not used. [Diagram 2] FIG. 2 is an illustration of an industrial robot operating in the vicinity of a vehicle body structure, where the vehicle body workpiece itself represents an obstacle that must be avoided, and where traditional collision avoidance motion planning techniques must again use dense waypoint spacing. [Diagram 3] FIG. 3 is an illustration of a sequence of waypoints through an obstacle environment according to one embodiment of the present disclosure, where a worst-case search identifies locations on the trajectory between each pair of waypoints that have a worst-case distance relative to the obstacles. [Figure 4] FIG. 4 is a diagram of the initial waypoints and trajectory of FIG. 3 according to one embodiment of the present disclosure, where worst-case condition optimization has moved the waypoint locations to improve the worst-case conditions to the point where collision avoidance criteria are satisfied. [Diagram 5] FIG. 5 is a flowchart diagram of a method for robot collision avoidance motion planning using worst-case condition search and waypoint optimization based on worst-case conditions according to one embodiment of the present disclosure. [Figure 6] FIG. 6 is a diagram of a robot collision avoidance motion planning system configured to use the method of FIG. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] The following description of embodiments of the present disclosure directed to robot collision avoidance motion planning techniques using worst-case search and worst-case optimization is merely exemplary in nature and is in no way intended to limit the disclosed apparatus and techniques or their applications or uses.

[0011] The use of industrial robots for various manufacturing, assembly, and material transfer operations is well known. In many robot workspace environments, obstacles are present that may be located in the path of motion of the robot - i.e., the obstacles may be located between where the robot is currently positioned and the destination location of the robot. Obstacles may be structures such as machines, fixtures, and tables, and the workpieces or containers being manipulated by the robot in and out of them may themselves be obstacles, since the robot must manipulate them in and out of them while performing its operations. Techniques have been developed in the art to compute the robot motion such that the tool follows a path while avoiding collision of the robot with any obstacles.

[0012] 1 is an illustration of an industrial robot operating in the vicinity of an obstacle where conventional collision avoidance motion planning techniques may fail to identify the obstacle collision if a sufficiently dense waypoint spacing is not used. The robot 100 is performing an action that may be as simple as moving a workpiece from a first container 110 to a second container 120. A divider 130 is located between the containers 110 and 120. The side walls of the containers 110 and 120 as well as the divider 130 all present obstacles that must be avoided by all parts of the robot 100, including the robot arms, the tool (which could be a finger-type grasper or a suction cup gripper), and the workpiece itself being moved.

[0013] The start point 140 and the goal point 142 represent the beginning and end points, respectively, on the path along which the workpiece will move. In workpiece pick-up and place operations of the type described above, it is common for a vision system (camera or other sensor - not shown) to identify the location within the container 110 of the next workpiece to be moved. It is also common for the destination location of the workpiece to vary from one to the next as the compartments within the container 120 are filled individually. Thus, both the start point 140 and the goal point 142 are uniquely identified for each workpiece to be moved, which means that a unique path must be computed for each workpiece. The start point 140 and the goal point 142, along with the geometric definitions of the obstacles (containers 110 and 120 and divider 130), are provided as inputs for the motion planning calculations.

[0014] It is known to define several waypoints along the path of the workpiece, where the initial reference locations of the waypoints may be evenly distributed along a straight line, for example from a start point 140 to a goal point 142. In prior art motion planning systems, a robot state is computed at each waypoint, and collision avoidance calculations are performed at each waypoint state. However, when the waypoints are sparsely spaced, the prior art motion planning system may fail to identify interference conditions that exist between the waypoints. In the following description, this situation will be illustrated with further reference to FIG. 1.

[0015] Trajectory 150 is defined as a straight line between start point 140 and goal point 142. In addition to start point 140 and goal point 142, two waypoints are defined: waypoint 152 and waypoint 154. In traditional optimization-based motion planning techniques, an initial reference path is defined, the robot state and interferences are calculated, and a new path is defined, which is iterated until a path is found where no collisions exist at any waypoint along the path. However, in the case of trajectory 150, it may be determined that no collisions occur at waypoint 152 or waypoint 154, and the motion planning system would conclude that straight line trajectory 150 is the appropriate path from start point 140 to goal point 142. This is clearly not an acceptable solution, as the robot outer arm and / or tool would collide with the sidewalls of container 110, divider 130, and sidewalls of container 120.

[0016] The above situation occurs because the prior art motion planning system only considers the distinct states represented by the waypoints and does not check for possible collisions between the waypoints. To avoid the above erroneous solution, users of the prior art motion planning system typically define a dense set of waypoints. For example, instead of the four points (start point 140, goal point 142, and two intermediate waypoints) used in trajectory 150, trajectory 160 is defined using a total of ten points (start point 140, goal point 142, and eight intermediate waypoints). Even if the intermediate waypoints (162, 164, 166, etc.) are initially arranged in a straight line between start point 140 and goal point 142, collisions will be identified in some of the distinct waypoint states, and the optimization routine will move the waypoints upward until they converge to trajectory 160 that arches around all obstacles between start point 140 and goal point 142.

[0017] 2 is an illustration of an industrial robot operating in the vicinity of a vehicle body structure, where the vehicle body workpiece itself represents an obstacle that must be avoided, and where traditional collision avoidance motion planning techniques must again use dense waypoint spacing. The robot 200 is performing an operation, such as spot welding, on the vehicle body structure 210. The start point 220 is the home location for the robot 200, and the goal point 222 is the location where the weld will be performed (e.g., on an interiorly disposed portion of the body structure 210). The roof rail 212 of the body structure 210 occupies the location directly between the start point 220 and the goal point 222.

[0018] 1 above, it is easy to envision how a straight-line trajectory 230 can be determined to be collision-free. Even if the trajectory 230 is defined by a moderate number of intermediate waypoints (such as five) between the start point 220 and the goal point 222, a prior art motion planning system would most likely fail to detect a collision because collision avoidance calculations are only performed at distinct waypoint states, and the thin roof rail 212 may pass between the waypoints.

[0019] If a large enough number of waypoints were defined, the prior art motion planning system would identify potential collisions with the roof rails 212. Collision avoidance constraints would cause the optimization routine to try different locations for the waypoints, and the waypoints would eventually converge to a trajectory 240 that passes outside the body structure 210 (between the base of the robot 200 and the body structure 210) and curves back to the goal point 222.

[0020] The dense waypoint spacing required for reliable collision avoidance in prior art motion planning systems is problematic for two reasons. First, optimization operations (including inverse kinematic robot configuration and interference checking calculations) become increasingly complex and time consuming as the number of waypoints increases. Second, it is difficult or impossible to achieve smooth motion of the robot tool with dense waypoint spacing because path segment transitions at each waypoint typically generate deceleration of the tool centerpoint due to the curvature required at the transition.

[0021] The following is a description of a collision avoidance motion planning method that does not require dense waypoint spacing, but reliably identifies collisions between waypoints along a continuous trajectory, and then optimizes waypoint locations to provide a trajectory that is collision-free and meets obstacle avoidance minimum distance requirements. In accordance with the techniques of this disclosure, the method includes a two-step worst-case search to identify locations on the continuous trajectory that have the worst interference conditions, followed by a two-step worst-case optimization that moves waypoints to improve the worst conditions until the collision avoidance requirements are met.

[0022] FIG. 3 is a diagram of a sequence of waypoints through an obstacle environment according to one embodiment of the present disclosure, where a worst-case search identifies a location on the trajectory between each pair of waypoints that has a worst-case distance relative to the obstacle. FIG. 3, along with FIG. 4 described below, are simply shown as two-dimensional (2D) for clarity. In an actual implementation, the coordinates of the obstacles, as well as the start and finish points, waypoints, and worst-case points, are all defined in three dimensions, and all of the calculations are processed accordingly. Also, the robot and workpiece are omitted from FIG. 3 and FIG. 4 for simplicity and clarity. It should be understood that the trajectories and waypoints in FIG. 3 and FIG. 4 are ultimately used by the robot to perform the operation, where the final trajectory is the path followed by the tool center point of the robot in the manner shown in FIG. 1 and FIG. 2.

[0023] In Figure 3, obstacles 310, 312, and 314 represent any type of object that must be avoided by the robot and workpiece. As discussed above in connection with Figures 1 and 2, obstacles 310 / 312 / 314 could be structures, tools, furniture, or the large workpiece itself. Human operators within the robot workspace could also be positioned within the safety zone defined by the obstacles.

[0024] A start point 320 and a goal point 322 are defined. The coordinates of each point shown in Figures 3 and 4 are defined in Cartesian space in a common fixed coordinate frame, such as the robot's work cell coordinate frame, or in terms of the robot's joint positions, as described further below. The coordinates of the start point 320 and the goal point 322 represent requirements for the operation to be performed, such as the position between which a part is moved, and therefore the locations of the start point 320 and the goal point 322 cannot be changed. Thus, the locations of the start point 320 and the goal point 322 are treated as equality constraints in the optimization calculations described below. An initial set of waypoints (332, 334, and 336) are generally defined at locations between the start point 320 and the goal point 322. The initial locations of the waypoints 332 / 334 / 336 may be defined in any suitable manner, including by a human operator using a device such as a teach pendant, automatic selection by a computer based on a pre-computed path with similar start and goal points, etc.

[0025] 3 illustrates two steps of the disclosed method: state parameterization and worst-state search. Each of these two steps is described below. State parameterization involves defining a continuous trajectory 330 as a function of a start point 320, a goal point 322, and waypoints 332 / 334 / 336. Some terminology is established as follows: The entire set of all points traversed by the trajectory 330 is denoted by q r That is, q r includes start and finish points 320 / 322 and waypoints 332 / 334 / 336. r= {q0, q1, q2, q3, q4}, where the start point 320 is q0, the goal point 322 is q4, and the waypoints 332 / 334 / 336 are q1, q2, q3, respectively. Each of the points such as q1 can be expressed in Cartesian space (x / y / z coordinates and roll / pitch / yaw) or in joint space (J1, J2, ... J nr In this case, n r are the degrees of freedom of the robot).

[0026] As a result, the continuous trajectory 330 is a function g(q r ,α), where α∈[0,1] is the arc-length parameter, and q r is the set of all endpoints and waypoints {q0, q1, q2, q3, q4} as described above. The superscript r means "reference" and signals the initial or previous value in the optimization calculation described below. As shown in FIG. 3, the waypoints are evenly spaced along the continuous trajectory 330 such that at q1, α=1 / 4, at q2, α=2 / 4, at q3, α=3 / 4, and at q4, α=4 / 4. To define the function g, a trajectory interpolation method must be selected. For example, the trajectory 330 can be defined as a linear interpolation, where the trajectory has the form of a straight line from q1 to q2, another straight line from q2 to q3, etc. The selection of an interpolation method for a set of waypoints is known in the art of robot motion programming.

[0027] In the case of linear interpolation of the locus 330, the function g in the section from q1 to q2 is evaluated by the following formula: g(q r ,α)=(1-α)q1+αq2(1) where all variables are as defined above. The function g can be evaluated in a similar manner in all other sections of the trajectory 330 (from q2 to q3, etc.).

[0028] Another trajectory interpolation method that can be used is spline interpolation. In one example of spline interpolation of trajectory 330, the function g in the section from q1 to q2 is evaluated by: g(q r ,α)=(kα 3 +k2α 2 +k1α+1)q1+(m3α 3 +m2α 2 +m1α)q2(2) In this case, k and m are constants that affect the shape of the spline trajectory, and again, the function g can be evaluated in a similar manner at all other sections (from q2 to q3, etc.) of the trajectory 330. Also, other types of trajectory interpolation besides linear and spline can be selected, as will be appreciated by those skilled in the art.

[0029] The above discussion completes the state parameterization step of the disclosed method. Optionally, the interpolation method (linear, spline, etc.) and the waypoints q r Once given, we define the continuous trajectory 330 as a function g(q r ,α), where α is the arc-length parameter.

[0030] The next step in the process, also shown in FIG. 3, is a worst-case search. The worst-case search is performed to find a waypoint q that has a worst-case distance in relation to an obstacle, which may be the worst amount of interference between the waypoint q and one of the obstacles 310 / 312 / 314 or the minimum distance to the waypoint q. r In the case of trajectory 330, worst-case points 352, 354, 356, and 358 are the worst-case of the first, second, third, and fourth sections of trajectory 330, respectively. Each of the worst-case points is worst,i where i=(1,...,4).

[0031] To find the worst case, we use waypoint q r It is possible to fix and formulate the one-dimensional minimization problem as

number

[0032] The minimization problem defined in equation (3) is solved for each of the trajectory sections. For the trajectory 330 of FIG. 3, α i * is found for each i=(1,...,4) of the sections. In FIG. 3, the function g uses spline interpolation, as can be seen by the significant curvature of the trajectory 330.

[0033] As a result, the initial set of assigned waypoints, q r The worst case at point α is expressed as follows: i * is identified by evaluating the function g at q worst,i =g(qr ,α i * ) (4) Here, q worst,i is the point in section i with the worst condition, and all other variables in equation (4) are as above.

[0034] The worst-case point q from equation (4) for i=(1,...,4) worst,i are the worst-case points 352, 354, 356, and 358 shown in FIG. 3 and described above. worst,1 ) has the greatest interference with obstacle 310. Worst case point 354 has the smallest distance to obstacle 312. Worst case points 356 and 358 both have the smallest distance to obstacle 314.

[0035] FIG. 3 and the above description are summarized as follows: In the state parameterization step, the interpolation method (linear, spline, etc.) and the waypoints q r Given that, the continuous trajectory 330 is a function g(q r ,α), where α is the arc-length parameter. Then, in a worst-state search step, the worst state in the successive trajectories is found using equations (3) and (4).

[0036] FIG. 4 illustrates a method for locating waypoint q of FIG. 3 in accordance with one embodiment of the present disclosure. r and trajectory 330, where the worst-case optimization moves waypoint locations to improve the worst-case conditions until the collision avoidance criteria are satisfied. Worst-case optimization consists of two steps: finding the relationship between the collision avoidance constraints and the waypoint locations, followed by the worst-case optimization itself. Both of these steps are depicted in FIG. 4 and are described below.

[0037] Location α i * With fixed, q from equation (4) worst,iWhen the value of is given, the worst-case point q wrost,i The inequality constraints in can be written as Dist(q worst,i ,Obs)=Dist(g(q,α i * ),Obs)≧d safe (5) where d safe where q is a predefined collision avoidance threshold criterion (a minimum safe clearance distance, such as 50 mm), and the only variable is the new set q of waypoint locations.

[0038] The objective of worst-case optimization is to find the initial location q r The solution is to move the waypoints from q to a new set of locations q. The first and last waypoints, q0 and q4, cannot be moved as described above. Only the intermediate waypoints can be moved to improve the worst state. To set up the optimization problem, the relationship between the collision avoidance constraints and the waypoint location q must be further refined. The partial derivatives of the worst state on the other waypoints are calculated to determine the relationship between the worst state and all the waypoints.

[0039] Using a first-order Taylor expansion with the chain rule, the worst-case metric value can be approximated as a linear combination of each of the waypoints. Thus, the relationship between the worst-case condition and each of the waypoints is approximated by the partial derivative of the collision metric as follows:

number

[0040] The above equation (5) is expressed as follows: i *and establishes the collision avoidance constraints in terms of the worst case conditions found at the new set q of waypoints. Equation (6) approximates the collision avoidance constraints in a form suitable for numerical solution within an optimization routine.

[0041] In the worst-case optimization step, an optimization problem is formulated having an objective function and one or more constraints. The objective function is typically chosen to achieve certain trajectory properties, such as minimum distance and / or smoothness. An example objective function is:

number

[0042] Several constraint functions can be defined for the optimization problem. One common constraint used in robot motion optimization is that the robot joints must stay within known joint position limits while moving the tool along the trajectory. The joint position limits can be defined as inequality constraints. Another constraint is that the start and finish points of the trajectory, points 320 and 322, must stay at the prescribed locations. The start and finish point locations can be defined as equality constraints. In addition to this, the worst-case collision metric, expressed in partial derivative form in equation (6) as described above, may be defined as an inequality collision avoidance constraint as follows:

number

[0043] With the worst-case optimization problem defined according to the above description of the objective function and constraints, different optimization update rules can be applied to update the positions of the waypoints. Possible optimization update rules include algorithms such as Sequential Quadratic Programming (SQP), SQP Trust Region, or Covariant Hamiltonian Optimization for Motion Planning (CHOMP). When the optimization calculation converges, a new set q of waypoint locations will result.

[0044] The collision avoidance inequality constraints included in the optimization calculations cause the optimization solver to "push" the worst-case conditions away from the obstacles 310 / 312 / 314, as indicated by the arrows in Figure 4. This has the resulting effect on the locations of the set q of new waypoint locations, which are also progressively moving in a direction that aids in mitigating the worst-case conditions to improve the collision metric.

[0045] The optimization calculations described in equations (7) and (8) have resulted in a new set q of waypoint locations that improve the obstacle avoidance situation relative to the worst-case conditions determined for the reference waypoint. Although the new trajectory will be relatively better in terms of obstacle avoidance than the previous trajectory, the new trajectory may still not satisfy the collision avoidance criteria. Thus, the process loops back to the beginning so that the new set q of waypoint locations are used as the reference waypoint input for the trajectory interpolation and worst-case condition search steps described above. This loop determines the set q of waypoint locations that satisfy the collision avoidance criteria, i.e., none of the worst-case distances are greater than the minimum collision avoidance threshold d safe The iteration continues until the time exceeds

[0046] In Figure 4, the new set q of waypoint locations after the final convergence to satisfy the collision avoidance criteria is shown. The final trajectory 360 starts at the start point 320, ends at the goal point 322, and passes through the new waypoints 362, 364, and 366. It can be observed that the final trajectory 360 avoids and passes through the obstacles 310 / 312 / 314. Again, it should be emphasized that the above calculations are all based on 3D geometry (not 2D as shown in Figures 3 and 4) and that the collision avoidance calculations take into account all parts of the robot (i.e., the robot arm and tool, as well as the tool center point path represented by the final trajectory 360).

[0047] 5 is a flowchart diagram 500 of a method for robot collision avoidance motion planning using worst-case search and waypoint optimization based on worst-case conditions according to one embodiment of the disclosure. In box 502, initialization is performed. Initialization includes defining the positions of the robot, start and goal points, and obstacles in a common coordinate frame, such as a workcell coordinate frame. Initialization also includes calculating the start and goal configurations of the robot using inverse kinematics based on the start and goal point locations.

[0048] In the interpolation step in box 504, reference waypoints from the start point to the goal point are provided and a robot configuration is computed at each waypoint using inverse kinematics. At the beginning of the process, the reference waypoint is an initial reference point (which may be, for example, a straight line from the start point to the goal point, a waypoint from a pre-planned path with similar start and goal points, or a point taught by an operator using a teach pendant device).

[0049] In box 506, state parameterization is performed in accordance with the above description. State parameterization includes defining a continuous trajectory as a function of the start point, goal point, and intermediate waypoints. The continuous trajectory is defined through the waypoints based on an interpolation method (linear, spline, etc.), and an arc-length parameter α is defined along the length of the trajectory. In box 508, a worst-case condition search is performed in accordance with the above description. The worst-case condition search includes finding a location along the trajectory between each adjacent pair of waypoints that has a worst-case distance relative to the obstacle. The worst-case condition search uses equation (3) to find the location α within each trajectory section where the worst-case condition occurs, and equation (4) to find the actual worst-case point within each trajectory section.

[0050] The state parameters in box 506 and the worst case search in box 508 collectively constitute two steps of the overall worst case search process contained in box 510 and depicted in FIG.

[0051] In box 512, a step of finding a relationship between the collision avoidance constraints and the waypoint locations using the worst-case conditions is performed in accordance with the above description. Finding the relationship includes using equations (5) and (6) above. In box 514, a worst-case optimization is performed in accordance with the above description. The worst-case optimization includes formulating an optimization problem having an objective function and one or more constraints, such as the example shown in equation (7). The constraints include a collision-avoidance minimum distance inequality constraint based on the relationship of the worst-case conditions to the waypoint locations, as shown in equation (8). The worst-case optimization has resulted in a new set of waypoints that improves the collision avoidance metric.

[0052] The steps of finding the relationship in box 512 and the worst-case optimization in box 514 collectively constitute two steps of the overall worst-case optimization process contained in box 516 and depicted on FIG.

[0053] Following the worst-case optimization, in decision diamond 518 it is determined whether the trajectory through the last set of waypoints satisfies a collision avoidance minimum distance criterion. If the minimum distance criterion is met, the trajectory through the last set of waypoints is used as a final or optimal trajectory in box 520. The final or optimal trajectory is used by the robot controller to control the motion of the robot to perform the desired operation (e.g., movement of a workpiece). If the minimum distance criterion is not met, the process loops from decision diamond 518 back to interpolation box 504 where the last set of waypoints is used as the reference waypoints to begin state parameterization again.

[0054] Figure 6 is a diagram of a robot collision avoidance motion planning system configured to use the worst case search and optimization method of Figure 5. A robot 600 is operating within a workspace 602 that includes one or more obstacles 610. A controller 620 is in communication with the robot 600, typically via a hard cable connection as shown. The controller 620 controls the motion of the robot 600 by sending joint motor commands to the robot 600 and receiving joint motor position data from joint encoders within the robot 600, as known in the art.

[0055] An optional computer 630 in communication with the controller 620 can be used for several different tasks, including providing obstacle shape data in the form of a CAD solid or surface model. The computer 630, if used, communicates with the controller 620 via any suitable wireless or wired network connection. As an alternative to using CAD data to define the obstacle 610, one or more sensors, such as the sensor 640, can be used. The sensor(s) 640 can be any type of camera or object sensor capable of providing a 3D shape of the obstacle 610 in the workspace 602. The sensor(s) 640 could be one or more 3D cameras, or multiple 2D cameras whose data are combined as 3D obstacle data. The sensor(s) 640 could also include other types of devices, such as radar, LiDAR, and / or ultrasonic. The sensor(s) 640 are also in communication with the controller 620 and / or the computer 630 via any suitable wireless or wired network connection.

[0056] In one embodiment, the controller 620 receives obstacle shape data from the computer 630 or one or more sensors 640. The obstacle shape data defines the 3D shape of all obstacles 610 present in the workspace 602. The controller 620 also determines the start and finish points for the next operation from the sensor data or otherwise. The controller 620 then proceeds to execute the remaining steps of the flowchart diagram 500, including interpolating the robot motion for the initial set of waypoints and performing worst case search and worst case optimization calculations until the collision avoidance criteria is satisfied. The controller 620 then computes the motion commands for the final trajectory and provides the commands to the robot 600.

[0057] In another embodiment, computer 630 performs most of the computations, including receiving obstacle data and start / goal points, interpolating initial waypoints, and performing worst-case search and worst-case optimization calculations until the collision avoidance criteria are satisfied. In this embodiment, computer 630 provides the final optimal trajectory and waypoints to controller 620, which computes and provides corresponding motion commands to robot 600. Computational responsibilities may be divided between computer 630 and controller 620 in any suitable manner.

[0058] Various computers and controllers are described and suggested throughout the above description. It should be understood that the software applications and modules of these computers and controllers execute on one or more computing devices having a processor and memory modules. In particular, this includes the processors within each of the robot controller 620 and computer 630 (if used) described above. In particular, the processors within the controller 620 and / or computer 630 (if used) are configured to perform collision avoidance path planning calculations using worst case search and worst case optimization in the manner described throughout the above disclosure.

[0059] As outlined above, the disclosed technique for robot collision avoidance motion optimization using worst-case search and worst-case optimization offers significant advantages over prior art methods: the disclosed worst-case search / worst-case optimization technique allows for sparse placement of waypoints while still finding and removing potential interference between waypoints, and possibly interference between thin obstacles that may be missed by prior art methods.

[0060] Although several example aspects and embodiments of the robot collision avoidance motion optimization technique using worst-case search and worst-case optimization have been described above, those skilled in the art will recognize modifications, permutations, additions, and sub-combinations thereof. Accordingly, the following appended claims and any claims hereafter introduced should be construed to include all such modifications, permutations, additions, and sub-combinations that fall within the true spirit and scope of the claims.

Claims

1. 1. A method for planning a path for an industrial robot, comprising: providing obstacle data defining obstacles within a workspace of the robot; defining a set of waypoints including a start point and a finish point for the route and one or more intermediate waypoints; computing a trajectory through the set of waypoints based on a defined path interpolation method, a trajectory section being defined as a portion of the trajectory between adjacent waypoints; defining an arc-length parameter as a distance along the trajectory; performing a worst-case search to identify a worst-case point within each trajectory section having a collision avoidance metric; performing a worst-case optimization using the worst-case points to determine a new set of waypoints; returning to the step of computing a trajectory using the new set of waypoints when the collision avoidance metric does not satisfy a predefined criterion; using the new set of waypoints and the corresponding optimal trajectory by a robot controller to control the motion of the robot when the collision avoidance metric satisfies the criterion; the path interpolation method includes one of linear interpolation, spline interpolation, or another type of non-linear interpolation; the collision avoidance metric is a minimum distance from any part of the robot to one of the obstacles, and the criterion is a minimum safe distance that the metric must exceed; performing the worst-case optimization step includes defining a relationship between the collision avoidance metric and the location of the waypoints based on the worst-case points; performing the worst case optimization step includes defining an objective function to be minimized and using the relationship to define collision avoidance inequality constraints; The method of claim 1, wherein the objective function includes a weighted combination of waypoint change distance, trajectory length, and trajectory shape terms, any of the weighting coefficients may be zero, and the optimization further includes equality constraints that determine the start and finish points.

2. 2. The method of claim 1, wherein providing obstacle data comprises providing the obstacle data from a computer-aided design (CAD) system or providing the obstacle data from one or more sensors configured to detect the obstacles in the workspace.

3. The method of claim 1 , wherein the start point and the goal point define a beginning and an end of the path of a robot tool center.

4. 2. The method of claim 1, wherein the arc-length parameter has a value of zero at the start point and a value of one at the goal point.

5. 2. The method of claim 1, wherein performing a worst-case search comprises finding the worst-case point along each trajectory section where the distance from any part of the robot to any of the obstacles is a minimum.

6. 2. The method of claim 1, wherein the relationship is approximated as a Taylor series expansion of the metric of the worst condition point as a linear combination of each of the waypoints, each waypoint term in the series comprising a partial derivative of the collision metric with respect to the trajectory defined as a function of the waypoints.

7. 1. A path planning system for an industrial robot, comprising: means for providing obstacle data defining obstacles within a workspace of said robot; a computer in communication with the robot and the means for providing obstacle data, defining a set of waypoints including a start point and a finish point of the route and one or more intermediate waypoints; computing a trajectory through the set of waypoints using a path interpolation function, a trajectory section being defined as a portion of the trajectory between adjacent waypoints; defining an arc-length parameter as a distance along the locus; performing a worst-case search to identify a worst-case point within each trajectory section having a collision avoidance metric; performing a worst-case optimization using the worst-case points to determine a new set of waypoints; returning to the step of computing a trajectory using the new set of waypoints when the collision avoidance metric does not satisfy a predefined criterion; using the new set of waypoints and the corresponding optimal trajectory to control the motion of the robot when the collision avoidance metric satisfies the criterion; the path interpolation method includes one of linear interpolation, spline interpolation, or another type of non-linear interpolation; the collision avoidance metric is a minimum distance from any part of the robot to one of the obstacles, and the criterion is a minimum safe distance that the metric must exceed; performing the worst-case optimization step includes defining a relationship between the collision avoidance metric and the location of the waypoints based on the worst-case points; performing the worst case optimization step includes defining an objective function to be minimized and using the relationship to define collision avoidance inequality constraints; the objective function includes a weighted combination of waypoint change distance, trajectory length, and trajectory shape terms, any of the weighting coefficients may be zero, and the optimization further includes equality constraints that determine the start and finish points. a computer having a processor and memory configured to A system having

8. 8. The system of claim 7, wherein the means for providing obstacle data comprises a computer-aided design (CAD) system or one or more sensors configured to detect the obstacles within the workspace, the one or more sensors comprising one or more cameras, radar sensors, LiDAR sensors, ultrasonic sensors, or infrared sensors.

9. 8. The system of claim 7, wherein the arc-length parameter has a value of 0 at the start point and a value of 1 at the goal point.

10. 8. The system of claim 7, wherein performing a worst-case search includes finding the worst-case point along each trajectory section where a distance from any part of the robot to any of the obstacles is a minimum.

11. The system of claim 7 , wherein performing the worst case optimization comprises defining a relationship between the collision avoidance metric and the waypoint locations based on the worst case points.

12. 12. The system of claim 11, wherein the relationship is approximated as a Taylor series expansion of the metric of the worst condition point as a linear combination of each of the waypoints, each waypoint term in the series including a partial derivative of the collision metric with respect to the trajectory defined as a function of the waypoints.

13. The system of claim 11 , wherein performing the worst-case optimization comprises: defining an objective function to be minimized; and using the relationship to define collision avoidance inequality constraints.

Images