Fast robot motion optimization with distance field

A path planning system for an industrial robot, said system comprising:

• means for providing obstacle data defining obstacles in a workspace of the robot;
• and
• a computer in communication with the robot and the means for providing obstacle data, said computer having a processor and memory configured for;
• computing a distance field matrix from the obstacle data, said distance field matrix being a three-dimensional (3D) matrix representing the workspace, where each cell in the distance field matrix contains a value designating a distance to a nearest cell occupied by any part of any of the obstacles;
• modeling a robot motion optimization problem, including defining an objective function, defining start and goal points of the path as equality constraints, and defining a collision avoidance inequality constraint using the distance field matrix, where defining the collision avoidance inequality constraint includes defining a plurality of points on arms of the robot, and determining, for each computed pose of the robot, a distance-to-obstacle for each of the points as the value in the cell containing the point minus a predefined radius for the point;
• and
• solving the motion optimization problem to yield the path of the robot.

A method for planning a path of an industrial robot, said method comprising:

• providing obstacle data defining obstacles in a workspace of the robot;
• computing a distance field matrix from the obstacle data, said distance field matrix being a three-dimensional (3D) matrix representing the workspace, where each cell in the distance field matrix contains a value designating a distance to a nearest cell occupied by any part of any of the obstacles;
• modeling a robot motion optimization problem, using a computer having a processor and memory, including defining an objective function, defining start and goal points of the path as equality constraints, and defining a collision avoidance inequality constraint using the distance field matrix, where modeling the robot motion optimization problem also includes defining motion limit inequality constraints, including robot joint rotational velocity, acceleration and jerk not exceeding predefined thresholds, robot tool center point velocity, acceleration and jerk not exceeding predefined thresholds, and joint positions not exceeding predefined limits, and further includes defining equality constraints based on system dynamics or kinematics equations for each joint of the robot;
• and
• solving the motion optimization problem, by the computer, to yield the path of the robot.

A path planning system for an industrial robot, said system comprising:

• means for providing obstacle data defining obstacles in a workspace of the robot;
• and
• a computer in communication with the robot and the means for providing obstacle data, said computer having a processor and memory configured for;
• computing a distance field matrix from the obstacle data, said distance field matrix being a three-dimensional (3D) matrix representing the workspace, where each cell in the distance field matrix contains a value designating a distance to a nearest cell occupied by any part of any of the obstacles;
• modeling a robot motion optimization problem, including defining an objective function, defining start and goal points of the path as equality constraints, and defining a collision avoidance inequality constraint using the distance field matrix, where modeling the robot motion optimization problem also includes defining motion limit inequality constraints, including robot joint rotational velocity, acceleration and jerk not exceeding predefined thresholds, robot tool center point velocity, acceleration and jerk not exceeding predefined thresholds, and joint positions not exceeding predefined limits, and further includes defining equality constraints based on system dynamics or kinematics equations for each joint of the robot;
• and
• solving the motion optimization problem to yield the path of the robot.