A Collision Avoidance Motion Planning Method for Industrial Robots

Abstract

A robot collision avoidance motion planning technique using worst-case state search and optimization. The motion planning technique begins with the geometric definition of obstacles, starting and goal points, and an initial set of waypoints that can be sparsely spaced. Given an interpolation method between points such as linear or spline, a continuous trajectory can be described as a function of waypoints and arc length parameters. Then, a worst-case state search is performed, which considers all parts and tools of the robot to find the position of the worst-case state with respect to the distance to obstacles between each pair of adjacent waypoints. Collision avoidance constraints are defined using the worst-case state positions, and then the waypoint positions are optimized to improve the worst-case state until all collisions are eliminated and the minimum distance criterion for obstacle avoidance is satisfied.

Description

[0001] Cross - Reference to Related Applications

[0002] This application claims the benefit of the priority date of U.S. Provisional Patent Application Serial No. 62 / 978,654, filed on February 19, 2020, and entitled COLLISION AVOIDANCE MOTION PLANNING METHOD FOR INDUSTRIAL ROBOT. Technical Field

[0003] The present disclosure relates to the field of industrial robot motion control, and more specifically, to a robot collision avoidance motion planning technique that starts with the definition of obstacles and an initial set of waypoints that may be sparsely spaced, finds the worst-case position with distance to the obstacle between each pair of adjacent waypoints, and then performs optimization of the waypoint positions to improve the worst-case state until a minimum collision avoidance distance criterion is met. Background Art

[0004] The use of industrial robots to perform a wide variety of manufacturing, assembly, and material movement operations is well known. In many robot workspace environments, obstacles are present and may be in the path of the robot's motion. Obstacles can be permanent structures such as machinery and fixtures. Obstacles can also be workpieces on which to operate or containers in which parts are placed, where the robot must maneuver in or around the workpiece or container while performing the operation. Collisions between the robot and any obstacles must be absolutely avoided.

[0005] Existing techniques for collision avoidance motion planning for robots typically involve defining a set of waypoints along a trajectory, and checking for collision or too small a clearance at the discrete state represented by each waypoint. In these techniques, collisions with small obstacles may be missed in the motion planning if the small obstacle happens to exist between the predefined waypoints. The only reliable way to ensure that small obstacles are not missed is to define a dense set of waypoints where the spacing between waypoints is not large enough to allow small obstacles to occupy them.

[0006] Using dense waypoint spacing as required in the prior art creates other problems. One problem with dense waypoint spacing is that it increases motion planning computation time because inverse kinematic robot configuration computations and collision avoidance optimization computations must be performed for each waypoint. Another problem with dense waypoint spacing is that it often causes uneven or jerky robot motion because deceleration is often required near each waypoint to perform curvature blending.

[0007] In view of the above, there is a need for an improved robot motion optimization technique that does not require dense waypoint spacing, but still reliably identifies and automatically resolves any collisions or minimum distance threshold violations along the robot trajectory. Summary of the Invention

[0008] In accordance with the teachings of the present disclosure, a robotic collision avoidance motion planning technique using worst-case state search and optimization is disclosed. The motion planning technique begins with a geometric definition of obstacles, start and goal points, and an initial set of waypoints that can be sparsely spaced. Given an interpolation method between points such as linear or spline, a continuous trajectory can be described as a function of waypoints and arc length parameters. Then a worst-case state search is performed, which considers all parts and tools of the robot to find the position of the worst-case state with respect to the distance to obstacles between each pair of adjacent waypoints. Collision avoidance constraints are defined using the worst-case state positions, and then the waypoint positions are optimized to improve the worst-case state until all collisions are eliminated and the minimum distance criterion for obstacle avoidance is satisfied.

[0009] In conjunction with the accompanying drawings, additional features of the presently disclosed apparatus and methods will become apparent from the following description and the appended claims. Brief Description of the Drawings

[0010] Figure 1 is an illustration of an industrial robot operating near an obstacle, where traditional collision avoidance motion planning techniques may not recognize an obstacle collision if an insufficiently dense waypoint spacing is used;

[0011] Figure 2 is an illustration of an industrial robot operating near 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 a dense waypoint spacing;

[0012] Figure 3 is an illustration of a sequence of waypoints through an obstacle environment according to an embodiment of the present disclosure, where the worst-case state search identifies positions on the trajectory that have the worst-case state distance relative to obstacles between each pair of waypoints;

[0013] Figure 4 is according to an embodiment of the present disclosure Figure 3 of the initial waypoints and trajectory, where the worst-case state optimization moves the waypoint positions to improve the worst-case state until the collision avoidance criteria are met;

[0014] Figure 5 is a flowchart of a method for robotic collision avoidance motion planning using worst-case state search and worst-case state-based waypoint optimization according to an embodiment of the present disclosure; and

[0015] Figure 6 is configured to use Figure 5 of the method of the robotic collision avoidance motion planning system illustration. Detailed Description

[0016] The following discussion of embodiments of the present disclosure relates to robotic collision avoidance motion planning techniques using worst-case search and worst-case optimization, which are merely exemplary in nature and are in no way intended to limit the disclosed devices and techniques or their application or use.

[0017] It is well known to use industrial robots for various manufacturing, assembly, and material handling operations. In many robotic workspace environments, there are obstacles and the obstacles may be in the path of the robot's motion, i.e., the obstacles may be located between where the robot is currently positioned and the destination location of the robot. Obstacles can be structures such as machines, fixtures, and workbenches, and the workpieces or containers themselves on or around which the robot operates can be obstacles because the robot must manipulate within or around the workpieces or containers when performing operations. Techniques for computing the motion of a robot have been developed in the art such that the tool follows a path while avoiding collisions of the robot with any obstacles.

[0018] Figure 1 FIG. is an illustration of an industrial robot working near obstacles, where traditional collision avoidance motion planning techniques may not be able to identify an obstacle collision if an insufficiently dense waypoint spacing is not used. The robot 100 is performing an operation that can be as simple as moving a workpiece from a first container 110 to a second container 120. A separator 130 is located between the containers 110 and 120. The sidewalls of the containers 110 and 120 and the separator 130 all represent obstacles that all parts of the robot 100 must avoid, and all parts of the robot 100 include all robotic arms, tools (which can be finger-type grippers or suction-type grippers), and the workpiece itself being moved.

[0019] The starting point 140 and the target point 142 represent the start and end points, respectively, on the path that the workpiece is to traverse. In a workpiece pick-and-place operation of the above type, the position of the next workpiece to be moved in the container 110 is typically identified by a vision system (camera or other sensor - not shown). When the compartments within the container 120 are filled individually, it is also common for the destination location of the workpiece to be different from one piece to the next. Thus, for each workpiece to be moved, the starting point 140 and the target point 142 are uniquely identified, which means that a unique path must be calculated for each workpiece. The starting point 140 and the target point 142, along with the geometric definition of the obstacles (containers 110 and 120, and separator 130), are provided as inputs to the motion planning calculation.

[0020] It is known that a plurality of waypoints are defined along a workpiece path, where, for example, the initial reference positions of the waypoints may be evenly distributed along a straight line from a starting point 140 to a target point 142. In prior art motion planning systems, the robot state is computed at each waypoint, and collision avoidance calculations are performed at each waypoint state. However, if the waypoints are sparsely spaced, the prior art motion planning system cannot recognize that interference situations existing between the waypoints are possible. Still with respect to Figure 1 , this situation is shown in the following discussion.

[0021] A trajectory 150 is defined as a straight line between a starting point 140 and a target point 142. In addition to the starting point 140 and the target point 142, two waypoints - waypoint 152 and waypoint 154 - are also defined. In traditional optimization-based motion planning techniques, an initial reference path is defined, the robot state and interference are computed, and a new path is defined; this is iteratively repeated until a path is found where no collision occurs at any waypoint along the path. However, in the case of the trajectory 150, it can be determined that no collision occurs at waypoint 152 or waypoint 154, and the motion planning system will infer that the straight-line trajectory 150 is a suitable path from the starting point 140 to the target point 142. This is clearly not an acceptable solution since the outer arm and / or tool of the robot will collide with the side wall of the container 110, the separator 130, and the side wall of the container 120.

[0022] The above situation occurs because prior art motion planning systems only consider the discrete states represented by the waypoints and do not check for possible collisions between the waypoints. To avoid the incorrect solutions as described above, users of prior art motion planning systems typically define a dense set of waypoints. For example, instead of using four points (starting point 140, target point 142, and two intermediate waypoints) as used in the trajectory 150, a total of ten points (starting point 140, target point 142, and eight intermediate waypoints) are used to define a trajectory 160. Even if the intermediate waypoints (162, 164, 166, etc.) are initially placed in the straight line between the starting point 140 and the target point 142, collisions will be recognized at some discrete waypoint states, and the optimization routine will move the waypoints upward until they converge to a trajectory 160 that arches upward and clears all obstacles between the starting point 140 and the target point 142.

[0023] Figure 2FIG. 0 is an illustration of an industrial robot working near 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 a dense waypoint spacing. The robot 200 performs operations such as spot welding on the vehicle body structure 210. The starting point 220 is the original position of the robot 200, and the target point 222 is the position where welding is to be performed (e.g., on a component located inside the body structure 210). The roof rail 212 of the vehicle body structure 210 occupies a position directly between the starting point 220 and the target point 222.

[0024] Based on the above discussion of Figure 1 it is easy to imagine how a straight-line trajectory 230 can be determined to be collision-free. Even if the trajectory 230 is defined to have a moderate number of intermediate waypoints (such as five) between the starting point 220 and the target point 222, an existing technology motion planning system is likely to be unable to detect a collision because the collision avoidance calculation is only performed at discrete waypoint states, and the thin roof rail 212 may pass between waypoints.

[0025] If a high enough number of waypoints are defined, an existing technology motion planning system will identify a potential collision with the roof rail 212. The collision avoidance constraint will cause the optimization routine to try different positions of the waypoints, and the waypoints will eventually converge to a trajectory 240 that passes around the outside of the body structure 210 (between the base of the robot 200 and the body structure 210) and bends back to the target point 222.

[0026] The dense waypoint spacing required to reliably avoid collisions in an existing technology motion planning system is problematic for two reasons. First, as the number of waypoints increases, the optimization calculations (including inverse kinematic robot configurations and interference checking calculations) become increasingly complex and time-consuming. Second, smooth motion of the robot tool is difficult or impossible to achieve with a dense waypoint spacing because the path segment transitions at each waypoint typically cause deceleration of the tool center point due to the required curvature at the transition.

[0027] A collision avoidance motion planning method is discussed below that does not require a dense waypoint spacing but reliably identifies collisions between waypoints along a continuous trajectory and then optimizes the waypoint positions to provide a collision-free trajectory that meets the minimum distance requirements for obstacle avoidance. According to the techniques of the present disclosure, the method includes a two-step worst-case state search to identify positions on the continuous trajectory with the worst interference conditions, followed by a two-step worst-case state optimization that moves the waypoints to improve the worst-case state until the collision avoidance requirements are met.

[0028] Figure 3Illustration of a sequence of waypoints through an obstacle environment according to an embodiment of the present disclosure, where worst-case state search identifies positions on the trajectory between each pair of waypoints that have the worst-case state distance relative to the obstacles. For clarity, Figure 3 is illustrated as two-dimensional (2D) together with Figure 4 discussed below. In a practical implementation, the coordinates of the obstacles, the start and target points, the waypoints, and the worst-case state points are all defined as three-dimensional, and all calculations are processed accordingly. For simplicity and clarity, Figure 3 and 4 the robot and the workpiece are also omitted. It should be understood that Figure 3 and Figure 4 the trajectories and waypoints of Figure 1 and Figure 2 are ultimately used by the robot to perform operations, where the final trajectory is the path adopted by the robot tool center point in the manner shown in

[0029] In Figure 3 , the obstacles 310, 312, and 314 represent any type of object that the robot and the workpiece must avoid. As described above with respect to Figure 1 and 2 , the obstacles 310 / 312 / 314 can be structures, tools, fixtures, or the large workpiece itself. A human operator in the robot's workspace can also be located in a safety area defined as an obstacle.

[0030] The start point 320 and the target point 322 are defined. Figure 3 and 4 The coordinates of each point shown in

[0031] Figure 3Depicts two steps of the disclosed method: state parameterization; and worst state search. Each of these two steps is discussed below. State parameterization involves defining a continuous trajectory 330 as a function of a starting point 320, a target point 322, and waypoints 332 / 334 / 336. Some terms are established as follows. The entire set of all points through which the trajectory 330 passes is defined as q r . That is, q r includes the starting and target points 320 / 322 and the waypoints 332 / 334 / 336. In other words, q r = {q0, q1, q2, q3, q4}, where the starting point 320 is q0, the target point 322 is q4, and the waypoints 332 / 334 / 336 are q1, q2, q3 respectively. Each point, such as q1, is defined in Cartesian space (x / y / z coordinates and roll / pitch / yaw) or in joint space ( where n r is the degree of freedom of the robot).

[0032] Then the continuous trajectory 330 is defined as a function g(g 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 represents "reference", which indicates the initial value or previous value in the optimization calculations discussed below. As Figure 3 shown, the waypoints are spaced equidistantly along the continuous trajectory 330 such that α = 1 / 4 at q1, α = 2 / 4 at q2, α = 3 / 4 at q3, and α = 4 / 4 at q4. To define the function g, a trajectory interpolation method must be selected. For example, the trajectory 330 can be defined as linear interpolation, where the trajectory is in the form of a straight line from q1 to q2, another straight line from q2 to q3, etc. Selecting an interpolation method for the set of waypoints is known in the field of robot motion programming.

[0033] In the case of linear interpolation of the trajectory 330, in the section from q1 to q2, the function g is evaluated as follows:

[0034] g(q r , α) = (1 - α)q1 + αq2 (1)

[0035] where all variables are defined as above. The function g can be evaluated in a similar manner in all other sections of the trajectory 330 (from q2 to q3, etc.).

[0036] Another trajectory interpolation method that can be used is spline interpolation. In one example of spline interpolation of the trajectory 330, in the section from q1 to q2, the function g is evaluated as follows:

[0037] g(q r , α) = (k3α 3 + k2α 2 + k1α + 1)q1 + (m3α 3 + m2α 2 + m1α)q2 (2)

[0038] where k and m are constants that affect the shape of the spline trajectory, and the function g can be evaluated in a similar manner in all other segments of the trajectory 330 (from q2 to q3, etc.). As will be understood by those skilled in the art, other types of trajectory interpolation methods other than linear and spline can also be selected.

[0039] The foregoing discussion completes the state parameterization step of the disclosed method. In summary, given an interpolation method (linear, spline, etc.) and waypoints q r , the continuous trajectory 330 can be described by the function g(q r , α), where α is the arc length parameter.

[0040] The next step in the process is the worst state search, which is also shown in Figure 3 . The worst state search involves finding the position along the trajectory 330 that has the worst state distance relative to the obstacle between each adjacent pair of waypoints q r - the worst state distance can be the worst interference amount with one of the obstacles 310 / 312 / 314, or the minimum distance to one of the obstacles 310 / 312 / 314. For the trajectory 330, the worst state points 352, 354, 356, and 358 are the worst states of the first, second, third, and fourth segments of the trajectory 330, respectively. Each of the above-mentioned plurality of worst state points is designated as q worst，i , where i = (1,..., 4).

[0041] To find the worst state, the waypoints q r are fixed, and the one-dimensional minimization problem can be formulated as follows:[[]]

[0042]

[0043] where is the position α in segment i where the worst state occurs, and the distance function Dist(g(q r , α), Obs) is evaluated by computing the minimum distance (or maximum interference) between all parts of the robot and the obstacles 310 / 312 / 314 for a specific point α on the trajectory 330 defined by the function g, where α is allowed to range from the lower bound (lb) of segment i to the upper bound (ub) of segment i. Thus, the minimization problem converges to the position in segment i where the worst state occurs The minimum distance (or maximum interference) between all parts of the robot and the obstacles 310 / 312 / 314 can be calculated at each point on the trajectory 330 using any suitable technique, such as establishing geometric primitives around each of the robot arm and the tool and calculating the distance from the primitives to the obstacles.

[0044] Solve the minimization problem defined in Equation (3) for each trajectory segment. For Figure 3 the trajectory 330, find for each of the segments i = (1,..., 4) In Figure 3 , as evidenced by the significant curvature of the trajectory 330, the function g uses spline interpolation.

[0045] Then identify the worst states of the given initial set of waypoints q by evaluating the function g at the points r as follows:

[0046]

[0047] where q worst，i is the point with the worst state in segment i, and all other variables in Equation (4) are described above.

[0048] The worst state points q worst，i , i = (1,..., 4), from Equation (4) are Figure 3 the worst state points 352, 354, 356, and 358 shown and discussed above. The worst state point 352 (q worst，1 ) has the maximum interference with the obstacle 310. The worst state point 354 has the minimum distance to the obstacle 312. The worst state points 356 and 358 both have the minimum distance to the obstacle 314.

[0049] Figure 3 And the foregoing discussion is summarized as follows. In the state parameterization step, given an interpolation method (linear, spline, etc.) and the waypoints q r , the continuous trajectory 330 is described by the function g(q r , α), where α is the arc length parameter. Then, in the worst state search step, Equations (3) and (4) are used to find the worst states in the continuous trajectory.

[0050] Figure 4 is a waypoint q Figure 3 according to an embodiment of the present disclosure rIllustration of the sum trajectory 330, where the worst-case state optimizes the positions of each waypoint to improve the worst-case state until the collision avoidance criteria are met. The worst-case state optimization consists of two steps; finding the relationship between the collision avoidance constraints and the positions of each waypoint, followed by the worst-case state optimization itself. These two steps are described in Figure 4 and discussed below.

[0051] Given the value of q from Equation (4), worst，i with the position fixed, the inequality constraints at the worst-case state point q worst，i can be written as:

[0052]

[0053] where d safe is a predefined collision avoidance threshold criterion (minimum safety clearance distance, e.g., 50 mm), and the only variable is the new set of waypoint positions q.

[0054] The goal of the worst-case state optimization is to move the waypoints from their initial positions q r to the new set of positions q. The first and last waypoints q0 and q4 cannot be moved as previously discussed. Only the intermediate waypoints can be moved to improve the worst-case state. To formulate the optimization problem, the relationship between the collision avoidance constraints and the waypoint positions q must be further refined. The partial derivatives of the worst-case state with respect to the other waypoints are calculated to determine the relationship between the worst-case state and all waypoints.

[0055] By using a first-order Taylor expansion with the chain rule, the measure of the worst-case state, which can be approximated as a linear combination of each waypoint, can be obtained. Therefore, the relationship between the worst-case state and each of the individual waypoints is approximated by the partial derivatives of the collision measure as follows:

[0056]

[0057] where the Dist function and the function g are described above, and the notation for different instances of q, including the superscript r representing the reference value from the previous iteration, is also discussed above.

[0058] Equation (5) above establishes the collision avoidance constraints based on the worst-case state found at the said position and the new set of waypoint positions q. Equation (6) approximates the collision avoidance constraints in a form suitable for numerical solution in the optimization routine.

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

[0060]

[0061] Among them, the w value is a weighting factor, ||q - q r || term represents the distance measure between the new waypoint and the reference waypoint, term represents an approximation of the path length passing through the waypoint, and term represents a curvature or smoothness measure. Other objective functions can be defined to suit specific applications.

[0062] Several constraint functions can be defined for the optimization problem. A typical constraint used in robot motion optimization is that when moving the tool along the trajectory, the robot joints must remain within known joint position limits. The joint position limits can be defined as inequality constraints. Another constraint is that the start and target points of the trajectory, namely points 320 and 322, must be maintained at the specified positions. The start and target point positions can be defined as equality constraints. Additionally, as described above, the worst-case collision measure represented in the form of partial derivatives in equation (6) can be defined as an inequality collision avoidance constraint as follows:

[0063]

[0064] where, in the above Taylor series expansion, j takes values from 0 to n, and i takes values from 1 to n, because j represents the waypoint and i represents the trajectory segment. For example, in Figure 3 and 4 n = 4 and there are five waypoints and four trajectory segments.

[0065] In the case of the worst-case optimization problem defined according to the foregoing discussion 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 SQP (Sequential Quadratic Programming), SQP trust region, or CHOMP (Covariant Hamiltonian Optimization for Motion Planning). When the optimization calculation converges, a new set of waypoint positions q will be obtained.

[0066] The collision avoidance inequality constraint included in the optimization calculation causes the optimization solver to "push" the worst-case state away from obstacles 310 / 312 / 314, as shown by the arrows in Figure 4 This has a final effect on each position in the new set of waypoint positions q, which also incrementally moves in a direction that helps to mitigate the worst-case state to improve the collision measure.

[0067] The optimization calculations described in equations (7) and (8) produce a new set of waypoint positions q that improves obstacle avoidance with respect to the worst-case state determined for the reference waypoints. Although the new trajectory will be better than the previous trajectory with respect to obstacle avoidance, the new trajectory may not yet meet the collision avoidance criteria. Therefore, the process loops back to the start such that the new set of waypoint positions q is used as the reference waypoint input for the trajectory interpolation and worst-case state search steps discussed previously. This loop is repeated until a set of waypoint positions q is determined that meets the collision avoidance criteria, i.e., all of the worst-case state distances exceed the minimum collision avoidance threshold d safe 。

[0068] In Figure 4 is shown the new set of waypoint positions q after final convergence to meet the collision avoidance criteria. 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 seen that the final trajectory 360 passes clear of the obstacles 310 / 312 / 314. Again, the above calculations are all based on three-dimensional geometry (not 2D as shown in Figure 3 and 4 ), and the collision avoidance calculations take into account all parts of the robot (i.e., the robot arm and tool, not just the TCP path represented by the final trajectory 360).

[0069] Figure 5 is a flowchart 500 of a method for robot collision avoidance motion planning using worst-case state search and worst-case state-based waypoint optimization according to an embodiment of the present disclosure. Initialization is performed at block 502. Initialization includes defining the positions of the robot, start and goal points, and obstacles in a common coordinate system such as a work cell coordinate system. Initialization also includes using inverse kinematics to compute the start and goal configurations of the robot based on the start and goal point positions.

[0070] In the interpolation step at block 504, reference waypoints are set from the start point to the goal point, and the robot configuration is computed at each waypoint using inverse kinematics. The first time through the process, the reference waypoints are the initially reference waypoints (e.g., it could be a straight line from the start point to the goal point, or waypoints from a previously planned path with similar start and goal points, or points taught by an operator using a teach pendant device).

[0071] At block 506, state parameterization is performed according to the previous discussion. State parameterization includes defining a continuous trajectory as a function of a starting point, a target 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. At block 508, worst state search is performed according to the previous discussion. Worst state search includes finding the positions along the trajectory that have the worst state distance relative to obstacles between each adjacent pair of the various waypoints. Worst state search uses Equation (3) to find the position α at which the worst state occurs in each trajectory segment, and uses Equation (4) to find the actual worst state point in each trajectory segment.

[0072] The state parameterization of block 506 and the worst state search of block 508 together constitute two steps of the entire worst state search process, which is included in block 510 and depicted in Figure 3 the above.

[0073] At block 512, finding the relationship between the collision avoidance constraint and the waypoint positions using the worst state is performed according to the previous discussion. Finding the relationship includes using the above Equations (5) and (6). At block 514, worst state optimization is performed according to the previous discussion. Worst state optimization includes formulating an optimization problem with an objective function such as the example shown in Equation (7) and one or more constraints. The constraints include a collision avoidance minimum distance inequality constraint based on the relationship between the worst state and the waypoint positions, as shown in Equation (8). Worst state optimization produces a new set of waypoints that improves the collision avoidance metric.

[0074] The finding the relationship of block 512 and the worst state optimization of block 514 together constitute two steps of the entire worst state optimization process, which is included in block 516 and depicted in Figure 4 the above.

[0075] After worst state optimization, at decision diamond 518, it is determined whether the trajectory passing through the latest set of waypoints satisfies the collision avoidance minimum distance criterion. If the minimum distance criterion is satisfied, then at block 520, the trajectory passing through the latest set of waypoints is used as the final or optimal trajectory. The robot controller uses the final or optimal trajectory to control the movement of the robot to perform the desired operation (e.g., movement of a workpiece). If the minimum distance criterion is not satisfied, the process loops back from decision diamond 518 to interpolation block 504, where the latest set of waypoints is used as the reference waypoints to start again with state parameterization.

[0076] Figure 6 is configured to use Figure 5Illustration of a robotic collision avoidance motion planning system with worst-case search and optimization methods. The robot 600 operates in a workspace 602 that includes one or more obstacles 610. The controller 620 generally communicates with the robot 600 via a hardwired cable connection as shown. As is well known in the art, 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 in the robot 600.

[0077] An optional computer 630 that communicates with the controller 620 can be used for several different tasks, including providing obstacle geometry data in the form of CAD entities or surface models. If the computer 630 is used, it communicates with the controller 620 via any suitable wireless or hardwired network connection. As an alternative to using CAD data to define the obstacles 610, one or more sensors such as the sensor 640 can be used. The sensor 640 can be a camera or any type of object sensor capable of providing the 3D geometry of the obstacles 610 in the workspace 602. The sensor 640 can be one or more 3D cameras, or multiple 2D cameras whose data is combined into 3D obstacle data. The sensor 640 can also include other types of devices such as radar, lidar, and / or ultrasonic. The sensor 640 also communicates with the controller 620 and / or the computer 630 via any suitable wireless or hardwired network connection.

[0078] In one embodiment, the controller 620 receives obstacle geometry data from the computer 630 or the sensor 640. The obstacle geometry data defines the 3D shape of all the obstacles 610 present in the workspace 602. The controller 620 also determines the start and target points of an upcoming operation based on sensor data or otherwise. The controller 620 then continues to execute the remaining steps of the flowchart 500, including interpolating the robot motion for an initial set of waypoints, and performing worst-case search and worst-case optimization calculations until a collision avoidance criterion is met. The controller 620 then calculates the motion commands for the final trajectory and provides the commands to the robot 600.

[0079] In another embodiment, the computer 630 performs most of the calculations, including receiving the obstacle data and start / target points, interpolating the initial waypoints, and performing worst-case search and worst-case optimization calculations until a collision avoidance criterion is met. In this embodiment, the computer 630 provides the controller 620 with the final best trajectory and waypoints, and the controller 620 calculates the corresponding motion commands and provides the commands to the robot 600. The computational responsibility can be divided between the computer 630 and the controller 620 in any suitable manner.

[0080] Throughout the foregoing discussion, various computers and controllers have been described and implied. 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 a memory module. In particular, this includes the processors in each of the robot controller 620 and the computer 630 (if used) discussed above. Specifically, the processors in the controller 620 and / or the 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 foregoing disclosure.

[0081] As described above, the disclosed robot collision avoidance motion optimization techniques using worst-case search and worst-case optimization provide significant advantages over prior art methods. The disclosed worst-case search / worst-case optimization techniques enable waypoints to be placed sparsely and still find and eliminate potential interferences between waypoints, even interferences with small obstacles that might be missed by prior art methods.

[0082] Although several exemplary aspects and embodiments of the robot collision avoidance motion optimization techniques using worst-case search and worst-case optimization have been discussed above, those skilled in the art will recognize modifications, permutations, additions, and sub-combinations thereof. Accordingly, the following appended claims and the claims introduced hereinafter are intended to be construed to include all such modifications, permutations, additions, and sub-combinations within their true spirit and scope.

Claims

1. A method for planning a path of an industrial robot, the method comprising: Providing obstacle data that defines a plurality of obstacles in the workspace of the robot; Defining a set of waypoints that includes a start point and a target point of the path and one or more intermediate waypoints; Calculating a trajectory passing through the set of waypoints based on a defined path interpolation method, wherein a trajectory segment is defined as a portion of the trajectory between adjacent waypoints; Defining an arc length parameter as the distance along the trajectory; Performing a worst-case state search to identify worst-case state points in each trajectory segment having a worst collision avoidance metric; Performing worst-case state optimization using the worst-case state points to determine a new set of waypoints, wherein performing the worst-case state optimization includes defining a relationship between the collision avoidance metric and the positions of the respective waypoints based on the worst-case state points, wherein performing the worst-case state optimization includes defining an objective function to be minimized and using the relationship to define collision avoidance inequality constraints, wherein the objective function includes a weighted combination of waypoint change distance, trajectory length, and trajectory shape terms, wherein any of the weighting factors can be zero, and the optimization further includes equality constraints that fix the start point and the target point; When the collision avoidance metric does not meet a predefined criterion, returning to calculate the trajectory using the new set of waypoints; and When the collision avoidance metric meets the criterion, controlling the movement of the robot by a robot controller using the new set of waypoints and the corresponding optimal trajectory, wherein the collision avoidance metric is the minimum distance from any part of the robot to one of the plurality of obstacles.

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

3. The method according to claim 1, wherein The start point and the target point define the start and end of the path of the robot tool center.

4. The method according to claim 1, wherein The path interpolation method includes one of linear interpolation, spline interpolation, or another type of non-linear interpolation.

5. The method according to claim 1, wherein, The arc length parameter has a value of zero at the start point and a value of one at the target point.

6. The method according to claim 1, wherein The criterion is a minimum safety distance that the collision avoidance metric must exceed.

7. The method according to claim 1, wherein Performing the worst-case state search includes finding the worst-case state points along each trajectory segment at which the distance from any part of the robot to any one of the plurality of obstacles is minimum.

8. The method according to claim 1, wherein The relationship is approximated as a Taylor series expansion of the metric of the worst-case state points, the metric of the worst-case state points being a linear combination of each of the respective waypoints, wherein each waypoint term in the series includes the partial derivative of the collision avoidance metric with respect to the trajectory defined as a function of the respective waypoints.

9. A method for planning a path of an industrial robot, the method comprising: Calculating a trajectory passing through a set of waypoints, wherein a trajectory segment is a portion of the trajectory between adjacent waypoints; Define the arc length parameter as the distance along the trajectory; perform a worst-case search to identify the worst-case state points with the worst collision avoidance metric in each trajectory segment; Perform worst-case optimization using the worst-case state points to determine a new set of waypoints, where performing the worst-case optimization includes defining the relationship between the collision avoidance metric and the positions of the respective waypoints based on the worst-case state points, where performing the worst-case optimization includes defining an objective function to be minimized and using the relationship to define collision avoidance inequality constraints, where the objective function includes a weighted combination of waypoint change distance, trajectory length, and trajectory shape terms, where any of the weighting factors can be zero, and the optimization further includes equality constraints for fixing the start point and the target point; and, return to calculate the trajectory using the new set of waypoints until the collision avoidance metric meets a predefined criterion, and when the collision avoidance metric meets the criterion, use the new set of waypoints and the corresponding best trajectory to control the movement of the robot, where the collision avoidance metric is the minimum distance from any part of the robot to one of the multiple obstacles.

10. A path planning system for an industrial robot, the system comprising: means for providing obstacle data that defines multiple obstacles in the workspace of the robot; and a computer that communicates with the robot and the means for providing obstacle data, the computer having a processor and a memory, the processor and the memory being configured to: define a set of waypoints that includes a start point and a target point of the path and one or more intermediate waypoints; calculate a trajectory passing through the set of waypoints using a path interpolation function, where a trajectory segment is defined as the portion between adjacent waypoints of the trajectory; define the arc length parameter as the distance along the trajectory; perform a worst-case search to identify the worst-case state points with the worst collision avoidance metric in each trajectory segment; perform worst-case optimization using the worst-case state points to determine a new set of waypoints, where performing the worst-case optimization includes defining the relationship between the collision avoidance metric and the positions of the respective waypoints based on the worst-case state points, where performing the worst-case optimization includes defining an objective function to be minimized and using the relationship to define collision avoidance inequality constraints, where the objective function includes a weighted combination of waypoint change distance, trajectory length, and trajectory shape terms, where any of the weighting factors can be zero, and the optimization further includes equality constraints for fixing the start point and the target point; when the collision avoidance metric does not meet the predefined criterion, return to calculate the trajectory using the new set of waypoints; and when the collision avoidance metric meets the criterion, use the new set of waypoints and the corresponding best trajectory to control the movement of the robot, where the collision avoidance metric is the minimum distance from any part of the robot to one of the multiple obstacles.

11. The system according to claim 10, wherein, The apparatus for providing obstacle data includes a computer-aided design (CAD) system or one or more sensors configured to detect the plurality of obstacles in the workspace, wherein the one or more sensors include one or more cameras, radar sensors, lidar sensors, ultrasonic sensors, or infrared sensors.

12. The system according to claim 10, wherein, The arc length parameter has a zero value at the starting point and a value at the target point.

13. The system according to claim 10, wherein The criterion is the minimum safe distance that the collision avoidance metric must exceed.

14. The system according to claim 10, wherein Performing a worst-case search includes finding the worst-case points along each trajectory segment, at which the distance from any part of the robot to any one of the plurality of obstacles is minimum.

15. The system according to claim 10, wherein, The relationship is approximated as a Taylor series expansion of the metric of the worst-case points, the metric of the worst-case points being a linear combination of each of the respective waypoints, wherein each waypoint term in the series includes the partial derivative of the collision avoidance metric with respect to the trajectory defined as a function of the respective waypoints.

Images