Safe motion planning for machinery operation
Systems and methods monitor a workspace for safety purposes using sensors distributed about the workspace. The sensors are registered with respect to each other, and this registration is monitored over time. Occluded space as well as occupied space is identified, and this mapping is frequently updated. Based on the mapping, a constrained motion plan of machinery can be generated to ensure safety.
1. A method of safely operating machinery in a three-dimensional workspace, the method comprising:
monitoring at least a portion of the workspace with a sensor associated with a grid of pixels for recording images of the at least a portion of the workspace within a field of view of the sensor;
based on the recorded images, computationally generating a three-dimensional representation of the at least a portion of workspace as a plurality of 3D volumes stored in a computer memory;
identifying volumes within the field of view of the sensor as occupied or unoccupied; and
mapping one or more safe volumetric zones within the workspace based at least in part on the identified volumes.
2. The method of claim 1 , wherein at least one said safe volumetric zone is mapped based at least in part on user input.
3. The method of claim 2 , wherein the at least one said safe volumetric zone is disposed outside of the field of view of the sensor.
4. The method of claim 2 , wherein the user input overrides a mapping based on images recorded by the sensor.
5. The method of claim 1 , wherein identifying a volume as occupied comprises receiving light reflected from an object disposed in the volume, an intensity of the reflected light being higher than a predetermined threshold.
6. The method of claim 1 , wherein one or more objects within the field of view of the sensor are insufficiently reflective to reflect light, back to the sensor, having an intensity higher than the threshold.
7. The method of claim 1 , further comprising associating the one or more safe volumetric zones with a safety criterion.
8. The method of claim 7 , wherein the safety criterion corresponds to a minimum distance from a piece of the controlled machinery.
9. The method of claim 1 , wherein the one or more safe volumetric zones include only the volumes that have been identified as unoccupied.
10. The method of claim 1 , further comprising, based on the recorded images, identifying a workpiece and/or at least an element of the machinery using a neural network.
11. The method of claim 1 , wherein identifying volumes within the field of view of the sensor as occupied or unoccupied comprises:
detecting an occlusion in the field of view of the sensor;
classifying the volumes associated with the occlusion as occupied volumes; and
classifying the volumes between the sensor and the occupied volumes in the field of view of the sensor as unoccupied volumes.
12. The method of claim 1 , wherein the sensor comprises at least one of a time-of-flight sensor, a 3D LIDAR sensor, or a stereo vision camera.
13. The method of claim 1 , further comprising:
monitoring at least a second portion of the workspace with one or more second sensors each associated with a grid of pixels for recording images of the at least a second portion of the workspace within a field of view of each second sensor; and
identifying volumes within the field of view of each second sensor as occupied or unoccupied.
14. The method of claim 1 , further comprising:
receiving a task specification; and
computationally generating a motion plan comprising at least one trajectory of the machinery to perform a specified task within the one or more safe volumetric zones.
15. The method of claim 14 , further comprising:
monitoring at least a second portion of the workspace with one or more second sensors each associated with a grid of pixels for recording images of the at least a second portion of the workspace within a field of view of each second sensor; and
identifying volumes within the field of view of each second sensor as occupied or unoccupied,
wherein mapping the one or more safe volumetric zones within the workspace is further based on the identified volumes within the field of view of each second sensor.
16. The method of claim 14 , further comprising operating the machinery in accordance with the motion plan.
17. The method of claim 14 , wherein the motion plan comprises a plurality of trajectories of the machinery, the method further comprising:
assigning a cost value to each of the trajectories;
selecting one of the trajectories based at least in part on the cost values assigned to the trajectories; and
causing the machinery to execute the selected trajectory.
18. The method of claim 17 , wherein the cost value associated with each of the trajectories is assigned based at least in part on a length of the trajectory, an operation time of the machinery on the trajectory, and/or allowed kinematics of the trajectory.
19. The method of claim 14 , wherein the motion plan precludes entry into a volumetric zone outside the one or more safe volumetric zones by the machinery and any workpiece associated with the machinery or the task.
20. The method of claim 14 , further comprising real-time monitoring of the workspace and altering the motion plan in response to a change in the one or more safe volumetric zones.