Robot obstacle collision prediction and avoidance
A method for predicting a collision between a mobile robot and an obstacle in an environment includes obtaining laser scan data for the mobile robot at a current location in the environment. The method also includes predicting a future location of the mobile robot in the environment and producing predicted laser scan data corresponding to the future location of the mobile robot in the environment. The method further includes assessing the predicted laser scan data relative to the mobile robot at the current location to determine whether a collision with an obstacle is predicted.
1. A method for predicting a collision between a mobile robot and an obstacle in an environment comprising:
generating a control trajectory for the mobile robot for each of a plurality of control cycles; and
predicting, between each of the plurality of control cycles, a collision of the mobile robot and an obstacle, wherein the step of predicting includes:
obtaining laser scan data for the mobile robot at a current location in the environment; wherein the laser scan data includes raw data output from a laser scanner and wherein the raw data output from the laser scanner includes laser scan points indicating points of reflection off of obstacles in the environment;
representing the mobile robot at the current location as a polygon;
predicting a future location of the mobile robot in the environment;
producing predicted laser scan data corresponding to the future location of the mobile robot in the environment, wherein the predicted laser scan data includes predicted laser scan points indicating predicted points of reflection off of obstacles in the environment from the future location of the mobile robot; and
assessing the predicted laser scan data relative to the mobile robot at the current location to determine whether a collision with an obstacle is predicted; wherein assessing the predicted laser scan data includes connecting each of the laser scan points with a corresponding predicted laser scan point with an arc, thereby forming a plurality of arcs, and determining if any of the plurality of arcs penetrate the polygon representing the mobile robot at the current location.
2. The method of claim 1 wherein the step of predicting the future location of the mobile robot in the environment includes estimating the future location of the mobile robot moving along an arc path after N seconds of travel from the current location using a commanded velocity of the mobile robot.
3. The method of claim 2 wherein N is a number between 1 and 2.
4. The method of claim 1 the polygon representing the mobile robot is an R-sided, convex polygon.
5. The method of claim 1 wherein each arc of the plurality of arcs comprises a plurality of line segments, L.
6. The method of claim 1 wherein the step of assessing the predicted laser scan data relative to the mobile robot at the current location includes determining if any of the plurality of arcs intersect with a point on the polygon representing the mobile robot, which is indicative of a potential collision between the mobile robot and an obstacle.
7. The method of claim 6 further including adjusting the commanded velocity of the mobile robot using a scaling factor based at least in part on a depth of incursion into the polygon for at least one intersecting arc.
8. The method of claim 7 wherein the depth of incursion into the polygon for each intersecting arc is determined based on the length of an arc length approximation for the intersecting arc.
9. The method of claim 8 wherein for each intersecting arc, a ratio of a straight line distance from the current laser scan point on the obstacle to the point of intersection on the polygon relative to the arc length approximation is determined and the minimum ratio is used as the scaling factor.
10. An autonomous mobile robot configured to predict a collision with an obstacle in an environment, comprising:
a mobile robot base;
a laser scanner mounted on the mobile robot base; and
a computer on the mobile robot base, including a processor and a memory, the computer being operatively coupled to the laser scanner; the processor configured to execute instructions stored in memory to:
generate a control trajectory for the mobile robot for each of a plurality of control cycles; and
predict, between each of the plurality of control cycles, a collision of the mobile robot and an obstacle, the step of predicting including:
obtaining laser scan data for the mobile robot at a current location in the environment; wherein the laser scan data includes raw data output from a laser scanner and wherein the raw data output from the laser scanner includes laser scan points indicating points of reflection off of obstacles in the environment;
representing the mobile robot at the current location as a polygon;
predicting a future location of the mobile robot in the environment;
producing predicted laser scan data corresponding to the future location of the mobile robot in the environment, wherein the predicted laser scan data includes predicted laser scan points indicating predicted points of reflection off of obstacles in the environment from the future location of the mobile robot; and
assessing the predicted laser scan data relative to the mobile robot at the current location to determine whether a collision with the obstacle is predicted; wherein assessing the predicted laser scan data includes connecting each of the laser scan points with a corresponding predicted laser scan point with an arc, thereby forming a plurality of arcs, and determining if any of the plurality of arcs penetrate the polygon representing the mobile robot at the current location.
11. The autonomous mobile robot of claim 10 wherein the instruction stored in memory to predict the future location of the mobile robot in the environment incudes estimating the future location of the mobile robot moving along an arc path after N seconds of travel from the current location using a commanded velocity of the mobile robot.
12. The autonomous mobile robot of claim 11 wherein N is a number between 1 and 2.
13. The autonomous mobile robot of claim 10 the polygon representing the mobile robot is an R-sided, convex polygon.
14. The autonomous mobile robot of claim 10 wherein each arc of the plurality of arcs comprises a plurality of line segments, L.
15. The autonomous mobile robot of claim 10 wherein when the processor executes instructions stored in memory to assess the predicted laser scan data relative to the mobile robot at the current location, the processor is further configured to determine if any of the plurality of arcs intersect with a point on the polygon representing the mobile robot, which is indicative of a potential collision between the mobile robot and an obstacle.
16. The autonomous mobile robot of claim 15 wherein the processor is further configured to execute instructions stored in memory to adjust a commanded velocity of the mobile robot using a scaling factor based at least in part on a depth of incursion into the polygon for at least one intersecting arc.
17. The autonomous mobile robot of claim 16 wherein the depth of incursion into the polygon for each intersecting arc is determined based on the length of an arc length approximation for the intersecting arc.
18. The method of claim 17 wherein the processor is further configured to execute instructions stored in memory to calculate a ratio for each intersecting arc, wherein the ratio is of a straight line distance from the current laser scan point on the obstacle to the point of intersection on the polygon relative to the arc length approximation and the minimum ratio is used as the scaling factor.