System and method for surface feature detection and traversal

1. A method for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), the TD traveling a path over a surface, the surface including the at least one SDSF, the path including a starting point and an ending point, the method comprising:

accessing point cloud data representing the surface;

forming the point cloud data into processable parts;

merging the processable parts into at least one concave polygon;

locating and labeling the at least one SDSF in the at least one concave polygon, the locating and labeling forming labeled point cloud data;

creating graphing polygons based at least on the at least one concave polygon comprising creating a convex polygon having exterior edges;

wherein one or more of the exterior edges has a weight based on: a directionality, a cost modifier, a drive mode, a current surface; an edge category, and combinations thereof; and

choosing the path from the starting point to the ending point based at least on the graphing polygons, the TD traversing the at least one SDSF along the path.

2. The method as in claim 1 further comprising filtering the point cloud data comprising:

conditionally removing points representing transient objects and points representing outliers from the point cloud data; and

replacing the removed points having a pre-selected height.

3. The method as in claim 1 wherein forming processable parts comprises:

segmenting the point cloud data into the processable parts; and

removing points of a pre-selected height from the processable parts.

4. The method as in claim 1 wherein the merging the processable parts comprises:

reducing a size of the processable parts by analyzing outliers, voxels, and normals;

growing regions from the reduced-size processable parts;

determining initial drivable surfaces from the grown regions;

segmenting and meshing the initial drivable surfaces;

locating polygons within the segmented and meshed initial drivable surfaces; and

setting at least one drivable surface based at least on the polygons.

5. The method as in claim 4 wherein the locating and labeling the at least one SDSF comprises:

sorting the point cloud data of the drivable surfaces according to a SDSF filter, the SDSF filter including at least three categories of points; and

locating at least one SDSF point based at least on whether the at least three categories of points, in combination, meet at least one first pre-selected criterion.

6. The method as in claim 5 further comprising:

creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF points, in combination, meet at least one second pre-selected criterion.

7. The method as in claim 6 wherein said creating graphing polygons further comprises:

smoothing the exterior edges;

forming a driving margin based on the smoothed exterior edges;

adding the at least one SDSF trajectory to the at least one drivable surface; and

removing interior edges from the at least one drivable surface according to at least one third pre-selected criterion.

8. The method as in claim 7 wherein the smoothing of the exterior edges comprises:

trimming the exterior edges outward forming outward edges.

9. The method as in claim 7 wherein forming the driving margin of the smoothed exterior edges comprises:

trimming the outward edges inward.

10. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a TD, the TD traveling a path over a surface, the surface including the at least one SDSF, the path including a starting point and an ending point, the system comprising:

sensors;

a map processor;

a power base;

a device controller, the device controller including:

a first processor configured for accessing point cloud data representing the surface;

a second processor configured for forming processable parts from the point cloud data;

a third processor configured for merging the processable parts into at least one concave polygon;

a fourth processor configured for locating and labeling the at least one SDSF in the at least one concave polygon, the locating and labeling forming labeled point cloud data;

a fifth processor configured for creating graphing polygons comprising creating a convex polygon having exterior edges; and

wherein one or more of the exterior edges has a weight based on: a directionality, a cost modifier, a drive mode, a current surface; an edge category, and combinations thereof;

a sixth processor configured for choosing the path from the starting point to the ending point based at least on the graphing polygons, the TD traversing the at least one SDSF along the path.

11. The system as in claim 10 further comprising a filter that comprises:

a seventh processor configured for executing code including:

conditionally removing points representing transient objects and points representing outliers from the point cloud data; and

replacing the removed points having a pre-selected height.

12. The system as in claim 10 wherein the second processor includes executable code configured for:

segmenting the point cloud data into the processable parts; and

removing points of a pre-selected height from the processable parts.

13. The system as in claim 10 wherein the third processor includes executable code configured for:

reducing a size of the processable parts by analyzing outliers, voxels, and normals;

growing regions from the reduced-size processable parts;

determining initial drivable surfaces from the grown regions;

segmenting and meshing the initial drivable surfaces;

locating polygons within the segmented and meshed initial drivable surfaces; and

setting at least one drivable surface based at least on the polygons.

14. The system as in claim 13 wherein the fourth processor includes executable code configured for:

sorting the point cloud data of the drivable surfaces according to a SDSF filter, the SDSF filter including at least three categories of points; and

locating at least one SDSF point based at least on whether the at least three categories of points, in combination, meet at least one first pre-selected criterion.

15. The system as in claim 14 wherein the fourth processor includes executable code configured for creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF points, in combination, meet at least one second pre-selected criterion.

16. The system as in claim 13 wherein creating graphing polygons includes an eighth processor including executable code comprising:

smoothing the exterior edges;

forming a driving margin based on the smoothed exterior edges;

adding the at least one SDSF trajectory to the at least one drivable surface; and

removing interior edges from the at least one drivable surface according to at least one third pre-selected criterion.

17. The system as in claim 16 wherein the smoothing the exterior edges includes a ninth processor including executable code configured for trimming the exterior edges outward forming outward edges.

18. The system as in claim 17 wherein forming the driving margin of the smoothed exterior edges includes a tenth processor including executable code configured for trimming the outward edges inward.

19. A method for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), the TD traveling a path over a surface, the surface including the at least one SDSF, the path including a starting point and an ending point, the method comprising:

accessing a route topology, the route topology including at least one graphing polygon comprising annotated point cloud data, the point cloud data including labeled features, the point cloud data including a drivable margin;

wherein:

the graphing polygon comprises a convex polygon, having exterior edges, based on a concave polygon based on merging processable parts from point cloud data; and

one or more of the exterior edges has a weight based on: a directionality; a cost modifier; a drive mode; a current surface; an edge category; and combinations thereof;

transforming the point cloud data into a global coordinate system;

determining boundaries of the at least one substantially discontinuous surface feature (SDSF);

creating SDSF buffers of a pre-selected size around the boundaries;

determining which of the at least one SDSFs can be traversed based at least on at least one SDSF traversal criterion;

creating an edge/weight graph based at least on the at least one SDSF traversal criterion, the transformed point cloud data, and the route topology; and

choosing the path from the starting point to the ending point based at least on the edge/weight graph.

20. The method as in claim 19 wherein the at least one SDSF traversal criterion comprises:

a pre-selected width of the at least one SDSF and a pre-selected smoothness of the at least one SDSF; and

a minimum ingress distance and a minimum egress distance between the at least one SDSF and the TD including a drivable surface,

wherein the minimum ingress distance between the at least one SDSF and the TD accommodating approximately a 90° approach by the TD to the at least one SDSF.