Deterministic robot path planning method for obstacle avoidance
The present teaching relates to a method and system for path planning. A target is tracked via one or more sensors. Information of a desired pose of an end-effector with respect to the target and a current pose of the end-effector is obtained. Also, a minimum distance permitted between an arm including the end-effector and each of at least one obstacle identified between the current pose of the end-effector and the target is obtained. A weighting factor previously learned is retrieved and a cost based on a cost function is computed in accordance with a weighted smallest distance between the arm including the end-effector and the at least one obstacle, wherein the smallest distance is weighted by the weighting factor. A trajectory is computed from the current pose to the desired pose by minimizing the cost function.
1. A method, comprising:
obtaining, via at least one sensor, a current pose of a multi-joint robotic arm;
calculating, via at least one processor, a minimum distance permitted between the multi-joint robotic arm and at least one obstacle identified between the current pose and a target;
calculating, via the at least one processor, a weighting factor, the weighting factor defining a relationship between a joint angle cost of joints within the multi-joint robotic arm and an obstacle-distance cost,
wherein the weighting factor is iteratively learned over a predetermined number of iterations by minimizing a cost function over a plurality of iterations using, for each iteration, a random desired position of an end-effector of the multi-joint robotic arm;
computing, via the at least one processor, a trajectory from the current pose to the target by minimizing the cost function using the weighting factor; and
initiating, via the at least one processor, movement of the multi-joint robotic arm along the trajectory.
2. The method of claim 1 , further comprising:
during the movement of the multi-joint robotic arm, detecting that the target is in motion; and
recalculating, via the at least one processor, the trajectory from the current pose to a new goal position.
3. The method of claim 1 , wherein the obtaining of the current pose of the multi-joint robotic arm via the at least one sensor is performed by at least one of camera-based vision tracking, optical sensor-based tracking, and magnetic sensor-based tracking.
4. The method of claim 1 , further comprising:
calculating, via the at least one processor based upon the trajectory, an actual minimum distance of the multi-joint robotic arm with respect to the target;
computing, via the at least one processor, an error corresponding to a difference between the actual minimum distance and the minimum distance; and
updating, via the at least one processor, the weighting factor based upon the error.
5. The method of claim 1 , wherein the minimum distance permitted between the multi-joint robotic arm and the at least one obstacle is determined based on a type of the at least one obstacle.
6. The method of claim 5 , wherein the type comprises at least one of a surgeon, a body part of a patient, and a medical device.
7. The method of claim 1 , further comprising, for each iteration in the predetermined number of iterations:
selecting an initial value of the weighting factor; and
minimizing the cost function based on the initial value.
8. A system comprising:
at least one sensor;
a multi-joint robotic arm;
at least one processor; and
a non-transitory computer-readable storage medium having instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
obtaining, from the at least one sensor, a current pose of the multi-joint robotic arm;
calculating a minimum distance permitted between the multi-joint robotic arm and at least one obstacle identified between the current pose and a target;
calculating a weighting factor, the weighting factor defining a relationship between a joint angle cost of joints within the multi-joint robotic arm and an obstacle-distance cost,
wherein the weighting factor is iteratively learned over a predetermined number of iterations by minimizing a cost function over a plurality of iterations using, for each iteration, a random desired position of an end-effector of the multi-joint robotic arm;
computing a trajectory from the current pose to the target by minimizing the cost function using the weighting factor; and
initiating movement of the multi-joint robotic arm along the trajectory.
9. The system of claim 8 , the non-transitory computer-readable storage medium having additional instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
during the movement of the multi-joint robotic arm, detecting, via the at least one sensor, that the target is in motion; and
recalculating the trajectory from the current pose to a new goal position.
10. The system of claim 8 , wherein the obtaining of the current pose of the multi-joint robotic arm via the at least one sensor is performed by at least one of camera-based vision tracking, optical sensor-based tracking, and magnetic sensor-based tracking.
11. The system of claim 8 , the non-transitory computer-readable storage medium having additional instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
calculating, based upon the trajectory, an actual minimum distance of the multi-joint robotic arm with respect to the target;
computing an error corresponding to a difference between the actual minimum distance and the minimum distance; and
updating the weighting factor based upon the error.
12. The system of claim 8 , wherein the minimum distance permitted between the multi-joint robotic arm and the at least one obstacle is determined based on a type of the at least one obstacle.
13. The system of claim 12 , wherein the type comprises at least one of a surgeon, a body part of a patient, and a medical device.
14. The system of claim 8 , the non-transitory computer-readable storage medium having additional instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
for each iteration in the predetermined number of iterations:
selecting an initial value of the weighting factor; and
minimizing the cost function based on the initial value.
15. A non-transitory computer-readable storage medium having instructions stored which, when executed by at least one processor, cause the at least one processor to perform operations comprising:
obtaining, from at least one sensor, a current pose of a multi-joint robotic arm;
calculating a minimum distance permitted between the multi-joint robotic arm and at least one obstacle identified between the current pose and a target;
calculating a weighting factor, the weighting factor defining a relationship between a joint angle cost of joints within the multi-joint robotic arm and an obstacle-distance cost,
wherein the weighting factor is iteratively learned over a predetermined number of iterations by minimizing a cost function over a plurality of iterations using, for each iteration, a random desired position of an end-effector of the multi-joint robotic arm;
computing a trajectory from the current pose to the target by minimizing the cost function using the weighting factor; and
initiating movement of the multi-joint robotic arm along the trajectory.
16. The non-transitory computer-readable storage medium of claim 15 , having additional instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
during the movement of the multi-joint robotic arm, detecting, via the at least one sensor, that the target is in motion; and
recalculating the trajectory from the current pose to a new goal position.
17. The non-transitory computer-readable storage medium of claim 15 , wherein the obtaining of the current pose of the multi-joint robotic arm via the at least one sensor is performed by at least one of camera-based vision tracking, optical sensor-based tracking, and magnetic sensor-based tracking.
18. The non-transitory computer-readable storage medium of claim 15 , having additional instructions stored which, when executed by the at least one processor, cause the at least one processor to perform operations comprising:
calculating, based upon the trajectory, an actual minimum distance of the multi-joint robotic arm with respect to the target;
computing an error corresponding to a difference between the actual minimum distance and the minimum distance; and
updating the weighting factor based upon the error.
19. The non-transitory computer-readable storage medium of claim 15 , wherein the minimum distance permitted between the multi-joint robotic arm and the at least one obstacle is determined based on a type of the at least one obstacle.
20. The non-transitory computer-readable storage medium of claim 19 , wherein the type comprises at least one of a surgeon, a body part of a patient, and a medical device.