Dynamic Motion Planning System

Abstract

To provide a method and a system of planning dynamic collision avoidance action for an industrial robot.SOLUTION: An obstacle avoidance action optimization routine receives a plan track and obstacle detection data as input, and calculates a robot track instruction for avoiding a detected obstacle. Robot joint action for following a tool center point track is used for instructing robot action by a robot control device. Planning and optimization calculation are performed by a feedback group separated from a feedback group of the control device calculating the robot instruction from an actual robot position. The two feedback groups perform planning, instruction and control calculation including response to a dynamic obstacle that may exists in a robot work space at real time. The optimization calculation includes a safety function effectively assembling both relative position of relative speed of the obstacle with respect to a robot.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present disclosure relates generally to the field of motion planning for industrial robots, and more particularly to methods and systems for motion planning for robots in the presence of dynamic obstacles, where a motion optimization calculation for obstacle avoidance is decoupled from the robot's feedback motion controller, and the motion optimization calculation includes obstacle avoidance constraints that effectively incorporate both the relative position and velocity of the obstacle with respect to the robot. [Background technology]

[0002] Industrial robots are widely known for performing a variety of manufacturing, assembly, and material handling tasks. In many robot workspace environments, obstacles may exist and be located in the robot's motion path. Obstacles may be permanent structures such as machinery or equipment that are easily avoided by the robot due to their static nature. Obstacles may also be dynamic objects that move into or randomly move within the robot's workspace. Real-time calculations for dynamic objects must be performed by the robot controller, and the robot must maneuver around the objects as it moves. Collisions between the robot and obstacles must be avoided at all costs.

[0003] Prior art motion planning for dynamic collision avoidance involves the computation of a collision avoidance safety function between a robot controller's motion planner and the physical robot system. In this computational arrangement, the robot's actual motion and obstacle detection and avoidance are included in a single closed-loop feedback system. While this arrangement makes sense in theory, in practice the system suffers from feedback delays due to the highly coupled and often contradictory nature of the input and feedback loops.

[0004] Additionally, existing dynamic motion planning systems use complex safety function configurations for collision avoidance. These systems exhibit various drawbacks, including numerous instability issues, insensitivity to static obstacles, computational complexity leading to slow motion planning calculations, and an inability to consider robot components other than the end-of-arm tool (i.e., the robot arm) in the robot-obstacle collision avoidance calculations. Summary of the Invention [Problem to be solved by the invention]

[0005] In view of the above, there is a need for an improved dynamic motion planning system for industrial robots that efficiently and effectively incorporates collision avoidance of static and dynamic obstacles. [Means for solving the problem]

[0006] In accordance with the teachings of the present disclosure, a method and system for dynamic collision avoidance motion planning for an industrial robot is described and demonstrated. An obstacle avoidance motion optimization routine receives as input a planned trajectory and obstacle detection data and calculates robot trajectory commands to avoid detected obstacles. Robot joint motions to track the tool center point trajectory are used by the robot controller to command robot motion. The planning and optimization calculations are performed in feedback loops separate from the controller feedback loop, which calculates robot commands from actual robot position. The two feedback loops perform real-time planning, command, and control calculations, including responses to dynamic obstacles that may be present in the robot workspace. The optimization calculation includes a safety function that effectively incorporates both the relative position and velocity of the obstacle with respect to the robot.

[0007] Additional features of the disclosed systems and methods will become apparent from the following description and appended claims, considered in conjunction with the accompanying drawings. [Brief explanation of the drawings]

[0008] [Figure 1]FIG. 1 is a block diagram of a closed-loop dynamic motion planning system known to those skilled in the art that incorporates a collision avoidance safety filter between the robot controller's motion calculations and the physical robot. [Figure 2] FIG. 2 is a block diagram of a dynamic motion planning system that includes a collision avoidance motion planning loop that is decoupled from the feedback control loop of the robot, according to an embodiment of the present disclosure. [Figure 3A] 3A and 3B illustrate two different motion scenarios of a robot and an obstacle, and the resulting configuration of safety functions for both, according to an embodiment of the present disclosure. [Figure 3B] 3A and 3B illustrate two different motion scenarios of a robot and an obstacle, and the resulting configuration of safety functions for both, according to an embodiment of the present disclosure. [Figure 4] FIG. 4 is a flowchart diagram of a method for dynamic robot motion planning according to an embodiment of the present disclosure. [Figure 5] FIG. 5 is a graph of the trajectory of a robot's tool center point in three-dimensional space, both when there is no obstacle in the robot workspace and when there is a moving obstacle in the workspace, in accordance with an embodiment of the present disclosure. [Figure 6] FIG. 6 is a graph of tool center point trajectories of multiple robots in three-dimensional space, each trajectory showing a different robot velocity, as the robots avoid static obstacles in the workspace, according to an embodiment of the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0009] The following discussion directed to dynamic motion planning systems, in accordance with embodiments of the present disclosure, is merely exemplary in nature and is in no way intended to limit the disclosed apparatus and techniques, or their applications or uses.

[0010] The use of industrial robots for various manufacturing, assembly, and material handling operations is widely known. In many robot workspace environments, obstacles may exist and may sometimes be located in the robot's motion trajectory. That is, without adaptive motion planning, some parts of the robot may collide with or come into close proximity with some parts of the obstacles as the robot moves from its current position to its destination position. Obstacles may be static structures such as machines, equipment, and desks, or they may be dynamic (moving) objects such as people, forklifts, and other machinery.

[0011] Techniques have been developed to calculate robot motion so that the tool can follow a trajectory to a desired location while avoiding collisions with obstacles. However, these systems exhibit various drawbacks, including numerous instability issues, insensitivity to static obstacles, computational complexity leading to slow motion planning, and an inability to consider robot components other than the end-of-arm tool (i.e., the robot arm) in the robot-obstacle collision avoidance calculations.

[0012] 1 is a block diagram of a closed-loop dynamic motion planning system known to those skilled in the art that incorporates a collision avoidance safety filter between the robot controller's motion calculations and the physical robot. The robot controller 110 calculates robot motion commands based on an input target (objective) position in a known manner. The motion commands are des (X) and u des is the acceleration vector of the tool center point that describes the "design" robot motion in Cartesian space. Rather than providing motion commands directly to the robot, the controller 110 provides motion commands to the safety filter module 120.

[0013] The safety filter module 120 also receives obstacle data input from a perception module 130. The perception module 130 includes one or more cameras or sensors that provide data about obstacles that may be present in the robot's workspace. The obstacle data typically includes the minimum distance between the robot and the obstacle, and may also include other data about the position (including spatial shape) and velocity of any obstacles. Based on the obstacle data, the safety filter module 120 generates corrective action commands u mod and provides a corrective motion command to the robot system 140. If no obstacles exist in the robot workspace, the corrective motion command u mod is the design operation instruction u des becomes the same as

[0014] The robot system 140 physically moves to generate corrective motion commands u mod The actual robot state vector X represents the position and velocity of the robot in Cartesian or joint space at the current control cycle time step. The actual robot state vector X is provided to both the controller 110 and the safety filter module 120 in a feedback loop 150, and the feedback control calculations are used to calculate the design motion commands u des and corrective action command u mod Each of these is used to calculate a new value to be used in the next control cycle time step.

[0015] In the feedback control arrangement of the prior art system shown in Figure 1, the controller 110 calculates the ideal motion command, and the safety filter 120, if necessary, modifies the ideal motion based on obstacle data. Both of these calculations involve feedback of the actual robot system state, and are theoretically meaningful. However, in practice, the highly connected and often contradictory nature of the input and feedback loops causes the system to experience feedback delays. In addition, existing dynamic motion planning systems of the type shown in Figure 1 often use complex safety function configurations in the safety filter module 120. These safety function configurations exacerbate the problem of slow motion plan calculations and introduce other issues discussed below.

[0016] The presently disclosed dynamic motion planning system overcomes the shortcomings of prior art systems by decoupling motion planning and obstacle avoidance calculations from the feedback control loop of the robot and its controller. The presently disclosed system also uses a simplified yet effective safety function construction that takes into account both the position and velocity of any obstacle relative to the robot when calculating the robot's motion commands.

[0017] 2 is a block diagram of a dynamic motion planning system that includes a collision avoidance motion planning loop that is decoupled from the robot's feedback control loop, according to an embodiment of the present disclosure. A planning module 210 calculates the planned robot motion based on an input target (objective) position. In one non-limiting example, the robot tool is a gripper, and the robot's task is to move a part from an original position to a target position. The planned robot motion u des is the acceleration vector that defines the "design" (planned) robot motion in Cartesian space. In particular, u des may be defined as the acceleration of the tool center point. The planning module 210 plans robot motion u des is provided to the dynamic motion optimization module 220.

[0018] The dynamic motion optimization module 220 also receives obstacle data input from a perception module 230. The perception module 230 includes one or more cameras or sensors that provide data about obstacles that may be present in the robot's workspace. As discussed above with respect to FIG. 1, the obstacle data typically includes at least the minimum distance between the robot and the obstacle, and may also include other data about the position (including spatial shape) and velocity of the obstacle.

[0019] The dynamic motion optimization module 220 calculates the planned robot motion u while including the robot's mechanical constraints and collision avoidance safety functions as constraints. des The optimization calculation is performed to minimize the tracking deviation from the robot. The result of this optimization calculation is the robot motion command q cmd Robot movement command qcmd is the robot's motion in joint space that moves the robot's tool to a target position while avoiding obstacles in the robot's workspace. The optimization calculation is discussed in detail below. The feedback loop 240 is cmd is provided from the dynamic motion optimization module 220 back to the planning module 210. The planning module 210 and the dynamic motion optimization module 220 repeat the above calculations at each control cycle.

[0020] The dynamic motion optimization module 220 also calculates the robot motion command q cmd to the robot controller 250. The robot controller 250 provides robot control instructions to the robot 260 and calculates the actual robot joint positions q in a feedback loop 270. act The robot controller 250 receives the robot control command in each control cycle and converts it into the actual robot joint position q act and robot operation command q cmd Update based on.

[0021] The dynamic motion planning system of FIG. 2 overcomes the feedback delay and computational performance problems of prior art systems by decoupling the motion planning and obstacle avoidance calculations (modules 210 and 220 and feedback loop 240) from the feedback control loop 270 of the robot 260 and its controller 250.

[0022] The actual hardware implementation of the dynamic motion planning system of Figure 2 may be done in two ways. In one implementation approach, the planning module 210 and the dynamic motion optimization module 220 are algorithms running on a processor within the robot controller 250. That is, the physical robot controller includes one or more processors that perform all of the calculations of modules 210, 220, and 250 of Figure 2 in the manner described above. In another implementation approach, the planning module 210 and the dynamic motion optimization module 220 send robot motion instructions q to the robot controller 250. cmdIt is an algorithm executed by a processor on another computer (different device) communicating with the

[0023] The dynamic motion optimization module 220 of the presently disclosed system also optimizes the robot motion command q cmd A simplified yet effective construction of a safety function is used that takes into account both the position and velocity of the obstacle relative to the robot when calculating . The construction of the safety function and its use in motion optimization calculations are discussed below.

[0024] 3A and 3B illustrate two different robot and obstacle motion scenarios and the resulting configuration of safety functions for both used within the dynamic motion optimization module 220, according to an embodiment of the present disclosure. In Figures 3A and 3B, a robot 300 with an end-of-arm tool 310 operates within a workspace. An obstacle 320 also exists within the workspace.

[0025] In FIG. 3A, the obstacle 320 moves away from the robot 300, so that the relative velocity of the obstacle 320 with respect to the robot 300 (i.e., the rate of change of the minimum distance) becomes greater than 0 (v rel >0). A positive relative velocity may be due to the obstacle 320 moving away from the robot 300, or due to the end-of-arm tool 310 moving away from the obstacle 320, or a combination of the two. As shown in FIG. 3A, the relative velocity v rel If is greater than 0, then the safety function is defined as h(X)=d, where h(X) is the safety function used in the inequality constraints of the optimization calculation, and d is the distance from the robot 300 to the obstacle 320 (typically the minimum distance determined by the perception module 230 in FIG. 2).

[0026] In FIG. 3B, the obstacle 320 approaches (approaches) the robot 300, so that the relative velocity of the obstacle 320 with respect to the robot 300 is less than or equal to 0 (v rel≦0). A negative relative velocity may be due to an obstacle 320 approaching the robot 300, or due to the end-of-arm tool 310 approaching the obstacle 320, or a combination of the two. As shown in FIG. 3B, the relative velocity v rel If is less than or equal to 0, the safety function is modified to include the relative velocity, resulting in h(X)=d-(v rel 2 ) / (2a max ), where h(X) is the safety function (used in the inequality constraints of the optimization calculation), d is the distance from the robot 300 to the obstacle 320, and v rel is the relative velocity, and a max is the maximum allowable robot acceleration based on mechanical constraints.

[0027] The safety function configuration depicted in Figures 3A and 3B and described above uses a simple calculation to less constrain robot motion when an obstacle moves away from the robot and to compensate for the closing velocity when the obstacle moves closer to the robot, both of which are simple and effective due to the consideration of relative velocity. The safety function is used within the inequality constraints (i.e., h(X) > 0) in the motion optimization calculations performed by the dynamic motion optimization module 220 of Figure 2. Details of the motion optimization calculations are discussed further below.

[0028] 4 is a flowchart diagram 400 of a method for dynamic robot motion planning, according to an embodiment of the present disclosure. In box 402, a planned robot motion is calculated based on a target or desired tool center point position. In an exemplary embodiment, the planned robot motion is a "design" (planned) robot motion u that moves the tool center point to the target position. desis the acceleration vector of the tool center point in Cartesian space, which defines: . In box 404, workspace obstacle data is provided by a perception module. The perception module includes at least one camera or sensor, such as a three-dimensional (3D) camera, that can detect the location of obstacles present in the workspace. The perception module may include an image processor that calculates obstacle position data from the camera images, or the perception module may simply provide raw camera images to a computer or controller that performs robot motion optimization calculations. The obstacle position data is preferably calculated in a workspace coordinate system that can be easily compared with the robot position data. The minimum robot-to-obstacle distance and the relative velocity of the robot to the obstacle are ultimately determined from the obstacle data.

[0029] In box 406, a robot motion optimization calculation is performed based on the planned robot motion and obstacle data. The output of the robot motion optimization calculation is the robot motion command q as discussed above with reference to FIG. cmd If there are no obstacles in the workspace, the robot motion commands are the same as the planned robot motion. The robot motion commands are provided in a feedback loop to box 402, where the planned robot motion is calculated based on the target tool center point position and the robot motion commands (modified during optimization to avoid obstacles).

[0030] In box 408, the robot controller provides robot movement instructions to the robot. The robot controller may perform calculations or transformations to provide appropriate robot joint movement instructions to the robot. In box 410, the robot actually moves based on the joint movement instructions from the controller. The robot and controller operate as a closed-loop feedback control system, and the actual robot state q act The joint positions and velocities are fed back to the controller for calculation of updated joint commands. The robot and controller operate in control cycles that have a specified time (i.e., a specific number of milliseconds).

[0031] In box 406, the motion optimization problem can be formulated as follows:

[0032]

number

[0033]

number

[0034]

number

[0035]

number

[0036]

number

[0037]

number

[0038]

number

[0039]

number

[0040]

number

[0041] For the obstacle avoidance constraint, the goal is to maintain the safety function h(X) ≥ 0 shown in Figures 3A and 3B and discussed above. This translates Equation 4 into

[0042]

number

[0043]

number

[0044]

number

[0045]

number

[0046] Upon convergence, the optimization calculation produces a robot motion command that satisfies the inequality constraints and minimizes the tracking error.

[0047]

number

[0048] 4, the calculation of the planned robot motion in box 402, the robot motion optimization calculation in box 406, and the calculation of the robot joint motion instructions in box 408 may all be performed by a robot controller in real-time communication with the robot. Alternatively, the calculations in boxes 402 and 406 may be performed on a separate computer, and the robot motion instructions for each control cycle may be provided to the controller in box 408.

[0049] The dynamic motion planning techniques of Figures 2 through 4 have been described to provide reliable obstacle avoidance results in realistic robotic systems. This includes both effective trajectory planning to avoid obstacles in the workspace and rapid computation of safety functions and resulting motion optimization.

[0050] 5 is a graph of a robot's tool center point trajectory in three-dimensional space, both when there are no obstacles in the robot workspace and when obstacles are moving in the workspace, in accordance with an embodiment of the present disclosure. As shown, workspace 500 is represented in 3D space with orthogonal X, Y, and Z axes. A robot (not shown) operates in workspace 500 and is required to perform the task of moving the tool center point from a start point 510 to a target (destination) point 512. FIG. 5 shows data collected during realistic laboratory testing.

[0051] Two different scenarios are shown in Figure 5. In the first scenario, there are no obstacles in the workspace 500. When there are no obstacles at all, the robot moves the tool center point in a straight line from a start point 510 to a target point 512 along a reference trajectory 520.

[0052] In the second scenario, an obstacle moves through workspace 500, tracing a series of points along obstacle trajectory 530. In a realistic laboratory test, the obstacle was a small object held by a human who approached the robot and extended their arm so that the object followed obstacle trajectory 530. The obstacle moved along obstacle trajectory 530 as the robot tool moved from start point 510 to goal point 512. Using the dynamic trajectory planning technique of the present disclosure, the robot moved its tool center point along obstacle-avoiding trajectory 540 from start point 510 to goal point 512. Obstacle-avoiding trajectory 540 deviates significantly from reference trajectory 520 to provide a safe gap from the obstacle moving along obstacle trajectory 530. It is noteworthy that the most significant deviation of obstacle-avoiding trajectory 540 from reference trajectory 520 is the initial portion of trajectory 540. This is because the obstacle is moving towards the robot tool at that time and the safety function h(X) is reduced to compensate for the approach speed as discussed with reference to Figure 3B.

[0053] Figure 6 is a graph of tool center point trajectories for multiple robots in three-dimensional space, each trajectory showing a different robot velocity as the robots avoid static obstacles in the workspace, in accordance with an embodiment of the present disclosure. As shown, workspace 600 is represented in 3D space with orthogonal X, Y, and Z axes. A robot (not shown) operates in workspace 600 and is required to perform the task of moving a tool center point from a start point 610 to a target (destination) point 612. Figure 6 shows data collected from a simulation of a particular robot and controller using the dynamic trajectory planning technique of the present disclosure.

[0054] Several different scenarios are shown in Figure 6. In the first scenario, there are no obstacles in the workspace 600. When there are no obstacles at all, the robot moves the tool center point in a straight line orthogonally along the nominal trajectory 620 from a start point 610 to a target point 612.

[0055] In another scenario, a fixed spherical obstacle 630 is placed within the workspace 600 at a position that interferes with the nominal trajectory 620. A buffer zone 640 defines a safe distance margin around the obstacle 630, and the robot and tool center points must be located outside the buffer zone 640. Four different simulations of the obstacle 630 were performed, using the same start point 610 and target point 612, and varying the programmed maximum tool center point speed of the robot from a slowest maximum of 850 mm / s to a fastest maximum of 1800 mm / s. The dynamic trajectory planning technique of the present disclosure was used to calculate the obstacle-avoiding trajectory shown in FIG. 6.

[0056] At the slowest robot tool center point velocity, 850 mm / s, the robot moved the tool center point from start point 610 to goal point 612 along obstacle-avoiding trajectory 652. It can be observed that obstacle-avoiding trajectory 652 deviates from nominal trajectory 620 enough to leave the tool center point slightly outside buffer zone 640. At a slightly faster robot tool center point velocity, 1000 mm / s, the robot moved the tool center point from start point 610 to goal point 612 along obstacle-avoiding trajectory 654. Obstacle-avoiding trajectory 654 deviates more from nominal trajectory 620 (than trajectory 652), leaving the tool center point further outside buffer zone 640. This is the expected behavior due to the subtraction term for the relative velocity in the safety function h(X) (shown in FIG. 3B). At the faster robot tool center point velocity, the relative velocity v between the tool and obstacle 630 rel becomes larger, the value of the safety function h(X) decreases, and instead the value of the minimum distance d increases to maintain the safety function h(X) ≥ 0 during the optimization calculation.

[0057] At the faster robot tool center point velocity of 1500 mm / s, the robot moved the tool center point along obstacle-avoiding trajectory 656 from start point 610 to destination point 612. Obstacle-avoiding trajectory 656 deviated more from nominal trajectory 620 (than trajectory 654), leaving the tool center point further outside buffer zone 640. And at the fastest robot tool center point velocity of 1800 mm / s, obstacle-avoiding trajectory 658 was taken, deviating the most from nominal trajectory 620.

[0058] Using the dynamic trajectory planning techniques of the present disclosure, the obstacle avoidance trajectory deviated the most from the nominal trajectory 620 at maximum tool speed (trajectory 658) and the least at minimum tool speed (trajectory 652). These simulations confirm the behavior expected from the safety function and motion optimization calculations discussed above.

[0059] In addition, the calculation time for the motion commands in each control cycle (the time until the output from the dynamic motion optimization module 220 is provided) in the disclosed technology depicted in Figures 2 through 4 was measured to be an average of 0.38 milliseconds (ms), which is a stark difference compared to the average calculation time of approximately 40 ms in the prior art of Figure 1. Using a typical robot control cycle of 24 ms, the technology of the present disclosure performs motion calculations much faster than is sufficient, while the motion calculations in the prior art are untenably slow.

[0060] Various computers and controllers have been described and alluded to throughout the preceding discussion. It will 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 robot controller 250 of FIG. 2 above and each of the optional separate computers (if used). In particular, the processors within controller 250 and / or the separate computers (if used) perform the robot feedback motion control function of box 250 as well as the motion planning and obstacle avoidance motion optimization functions of boxes 210 and 220.

[0061] As outlined above, the disclosed technique for dynamic motion planning to avoid obstacles in a robot workspace provides significant advantages over prior art methods. The disclosed technique decouples obstacle avoidance motion optimization from the robot and controller feedback loop, thereby avoiding the feedback delay problems of prior art systems. In addition, the disclosed safety function used in the motion optimization calculations compensates for the relative velocity of the robot and obstacles in an efficient and easily computed manner.

[0062] While several example aspects and embodiments of the dynamic motion planning system have been discussed above, those skilled in the art will recognize modifications, permutations, additions, and sub-combinations, and it is therefore intended that the following appended and hereafter introduced claims be interpreted to include all such modifications, permutations, additions, and sub-combinations that are within the true spirit and scope of the present disclosure.

Claims

1. A dynamic motion planning system for an industrial robot, comprising: a perception module including at least one sensor or camera for detecting obstacles in the workspace of the industrial robot; a planning module, executing on a first computing device having a processor and a memory, that calculates a design robot motion based on a target position of a tool center point on a tool attached to the tip of the industrial robot; a motion optimization module executing on the first computing device and receiving obstacle data from the perception module, the motion optimization module calculating robot motion instructions based on the designed robot motion and the obstacle data, the robot motion instructions being provided as feedback to the planning module to calculate the designed robot motion for a next control cycle; and a robot control module running on the first computing device or the second computing device, which calculates robot control instructions based on the robot movement instructions, provides the robot control instructions to the industrial robot, and receives actual robot state data as feedback from the industrial robot; A dynamic motion planning system for an industrial robot, comprising:

2. 2. The system of claim 1, wherein the first computing device is a robot controller that executes all of the planning module, the motion optimization module, and the robot control module and provides the robot control instructions to the industrial robot.

3. 2. The system of claim 1, wherein the first computing device executes the planning module and the motion optimization module, and the second computing device is a robot controller in communication with the first computing device, the robot controller executing the robot control module and providing the robot control instructions to the industrial robot.

4. The system of claim 1 , wherein the robot movement instructions deviate from the design robot movement as necessary to provide clearance space between the industrial robot and obstacles present in the obstacle data.

5. The system of claim 1 , wherein the motion optimization module calculates the robot motion instructions using an iterative optimization calculation having an objective function and one or more inequality constraints.

6. 6. The system of claim 5, wherein the objective function minimizes deviation from the designed robot motion and the inequality constraints include maintaining robot joint positions within predefined joint position ranges, maintaining robot joint velocities below predefined joint velocity limits, and maintaining robot joint accelerations below predefined joint acceleration limits.

7. The system of claim 5 , wherein the inequality constraints include safety constraints calculated from a safety function, the safety function being determined based on a minimum distance between the robot and an obstacle and a relative velocity between the robot and the obstacle.

8. 8. The system of claim 7, wherein the safety constraint is that the rate of change of the safety function must be greater than or equal to the negative of the safety function multiplied by a coefficient.

9. 8. The system of claim 7, wherein when the relative velocity between the robot and the obstacle is greater than zero, the safety function is equal to the minimum distance between the robot and the obstacle, and when the relative velocity between the robot and the obstacle is less than or equal to zero, the safety function is equal to the minimum distance between the robot and the obstacle minus an additional safety margin distance that is a function of the relative velocity between the robot and the obstacle.

10. The system of claim 1 , wherein the designed robot motion is an acceleration of a tool center point in Cartesian space, and the robot motion command includes robot joint rotation angular accelerations of all joints of the industrial robot.

11. A dynamic motion planning system for an industrial robot, comprising: the dynamic motion planning system executes on one or more computing devices; a planning module that calculates a design robot motion based on a target position of the tool center point of the robot; a motion optimization module that calculates robot motion commands based on the designed robot motion and obstacle data received from sensors, the robot motion commands being provided as feedback to the planning module to calculate the designed robot motion for a next control cycle; and a robot control module that calculates a robot joint control command based on the robot movement command, provides the robot joint control command to the industrial robot, and receives actual robot joint state data as feedback from the industrial robot; A dynamic motion planning system for an industrial robot, comprising:

12. 12. The dynamic motion planning system of claim 11, wherein the motion optimization module calculates the robot motion commands using an iterative optimization calculation having an objective function that minimizes deviation from the designed robot motion and safety constraints determined based on a minimum distance between the robot and an obstacle and a relative velocity between the robot and the obstacle, and wherein the safety constraints are relaxed more when the relative velocity between the robot and the obstacle is greater than zero than when the relative velocity is less than or equal to zero.

13. Executed on at least one computing device, Calculating a design robot motion based on a target position of a tool center point of the robot; calculating robot movement commands based on the designed robot movement and obstacle data received from sensors, the robot movement commands being provided as feedback to calculate the designed robot movement for a next control cycle; calculating a robot joint control command based on the robot movement command; providing the robot joint control instructions to the robot; receiving actual robot joint state data as feedback from said robot; A dynamic motion planning method for an industrial robot, comprising:

14. 14. The method of claim 13, wherein the at least one computing device is a robot controller that performs all of: calculating a design robot motion; calculating robot motion instructions; calculating robot joint control instructions; and providing the robot joint control instructions to the robot.

15. 14. The method of claim 13, wherein a first computing device computes the designed robot motion and computes the robot motion instructions, and a robot controller computes the robot joint control instructions and provides the robot joint control instructions to the robot.

16. 14. The method of claim 13, wherein the robot movement commands are calculated using an iterative optimization algorithm having an objective function and a plurality of inequality constraints, the objective function minimizing deviations from the design robot movement, and the inequality constraints including maintaining robot joint positions within predefined joint position ranges, maintaining robot joint velocities below predefined joint velocity limits, and maintaining robot joint accelerations below predefined joint acceleration limits.

17. 17. The method of claim 16, wherein the inequality constraints also include safety constraints calculated from a safety function, the safety function being determined based on a minimum distance between the robot and an obstacle and a relative velocity between the robot and the obstacle.

18. 18. The method of claim 17, wherein the safety constraint is that the rate of change of the safety function must be greater than or equal to the negative of the safety function multiplied by a coefficient.

19. 18. The method of claim 17, wherein when the relative velocity between the robot and the obstacle is greater than zero, the safety function is equal to the minimum distance between the robot and the obstacle, and when the relative velocity between the robot and the obstacle is less than or equal to zero, the safety function is equal to the minimum distance between the robot and the obstacle minus the square of the relative velocity divided by twice the robot acceleration.

20. The method of claim 13 , wherein the designed robot motion is an acceleration of a tool center point in Cartesian space, and the robot motion command includes robot joint rotation angular accelerations for all joints of the robot.

Images