Systems and methods for surface feature detection and traversal

Abstract

To provide a system and a method for surface feature detection and traversal.SOLUTION: A substantially discontinuous surface feature traversal feature of the present teachings can be utilized by a transportation device 101, such as, but not limited to, an autonomous or semi-autonomous device, to navigate within an environment that may include features, such as the subsequently discontinuous surface features or the like. The substantially discontinuous surface feature traversal feature can enable the transportation device 101 to navigate over an extended variety of surfaces. In particular, the substantially discontinuous surface feature can be precisely identified and labeled such that the transportation device 101 can automatically maintain the performance of the transportation device 101 during the ingress and egress of the substantially discontinuous surface features.SELECTED DRAWING: Figure 1

Description

[Background technology]

[0001] (CROSS-REFERENCE TO RELATED APPLICATIONS) This utility patent application claims priority to U.S. Provisional Patent Application No. 62 / 809,973 (Attorney Docket No. Z26), filed February 25, 2019, entitled "System and Method for Surface Feature Detection and Traversal," which are incorporated herein by reference in their entireties; U.S. Provisional Patent Application No. 62 / 851,266 (Attorney Docket No. Z81), filed May 22, 2019, entitled "System and Method for Surface Feature Traversal," and U.S. Provisional Patent Application No. 62 / 851,266 (Attorney Docket No. Z88), filed May 23, 2019, entitled "System and Method for Surface Feature Traversal."

[0002] The present teachings generally relate to surface feature detection and traversal. Surface feature traversal is challenging because surface features, such as, but not limited to, substantially discontinuous surface features (SDSFs), can be found in heterogeneous forms, and their forms can be unique to specific geographies. However, SDSFs, such as, but not limited to, slopes, edges, curbs, steps, and curb-like geometries (referred to herein, in a non-limiting manner, as SDSFs or simply surface features), can include several typical characteristics that can aid in their identification.

[0003] A wide range of devices and methods are known for transporting people and cargo, including autonomous transportation. These device designs address uneven driving surfaces in several different ways. However, what is lacking is the ability to localize an SDSF based on a multipart model associated with several criteria for SDSF identification. Also lacking is the integration of the localized SDSF trajectory with a graphed polygon that can form a route configuration. Furthermore, the determination of candidate surface feature traversal does not rely on criteria such as candidate traversal approach angle, candidate traversal driving surfaces on both sides of the candidate surface feature, and candidate traversal path obstacles. Summary of the Invention [Means for solving the problem]

[0004] The SDSF traversal of the present teachings can utilize a transport device (TD), such as, but not limited to, an autonomous or semi-autonomous device, to navigate within an environment that may include features such as an SDSF. The SDSF traversal features can enable the TD to navigate over an extended variety of surfaces. In particular, the SDSF can be precisely identified so that the TD can automatically maintain its performance during the traversal of the SDSF. In some configurations, the SDSF can be identified by its dimensions. For example, but not limited to, a curb may include a width of approximately 0.6-0.7 m. In some configurations, point cloud data can be processed to identify the location of the SDSF, and these data can be used to prepare a route for the TD from the starting point to the destination. In some configurations, the SDSF traversal can be adapted through sensor-based positioning of the TD while the TD is navigating the route.

[0005] In some configurations, a method of the present teachings for navigating at least one SDSF encountered by a TD, wherein the TD travels a path on a surface, the surface including at least one SDSF, the path including a start point and an end point, can include, without limitation, accessing point cloud data representing the surface, filtering the point cloud data, forming the filtered point cloud data into a processable portion, and merging the processable portion into at least one concave polygon. The method can include locating and labeling the at least one SDSF within the at least one concave polygon. The locating and labeling can form the labeled point cloud data. The method can include creating a graphing polygon based at least on the at least one concave polygon, and selecting a path from the start point to the end point based at least on the graphing polygon. The TD can traverse the at least one SDSF along the path.

[0006] Filtering the point cloud data can optionally include conditionally removing points representing transient objects and outliers from the point cloud data and replacing the removed points with a preselected height. Forming the processing portions can optionally include dividing the point cloud data into processable portions and removing points at the preselected height from the processable portions. Merging the processable portions can optionally include reducing a size of the processable portions by analyzing outliers, voxels, and normals, expanding a region from the reduced-sized processable portions, determining an initial drivable surface from the expanded region, dividing and meshing the initial drivable surface, locating polygons within the divided and meshed initial drivable surface, and setting the drivable surface based at least on the polygons. Locating and labeling at least one SDSF feature can optionally include sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points, and locating the at least one SDSF point based at least on whether the categories of points, in combination, satisfy at least one first preselected criterion. The method can optionally include creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF point, in combination, satisfies at least one second preselected criterion. Creating a graphing polygon can further optionally include creating at least one convex polygon from the at least one drivable surface. The at least one convex polygon can include an edge. Creating a graphing polygon can include smoothing the edge, forming a driving margin based on the smoothed edge, adding the at least one SDSF trajectory to the at least one drivable surface, and removing the edge from the at least one drivable surface according to at least one third preselected criterion. Smoothing the edges can optionally include trimming the edges outward.Providing a smoothed edge running margin can optionally include trimming the outer edge inward.

[0007] In some configurations, a system of the present teachings for navigating at least one SDSF encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, can include, without limitation, a first processor that accesses point cloud data representing the surface, a first filter that filters the point cloud data, a second processor that forms a processable portion from the filtered point cloud data, a third processor that merges the processable portion into at least one concave polygon, a fourth processor that locates and labels the at least one SDSF within the at least one concave polygon, where the locating and labeling forms the labeled point cloud data, a fifth processor that creates a graphing polygon, and a path selector that selects a path from the start point to the end point based on at least the graphing polygon. The TD can traverse the at least one SDSF along the path.

[0008] The first filter may optionally include executable code that may include, without limitation, conditionally removing points representing transient objects and points representing outliers from the point cloud data and replacing the removed points with a preselected height. The segmenter may optionally include executable code that may include, without limitation, dividing the point cloud data into processable portions and removing points at a preselected height from the processable portions. The third processor may optionally include executable code that may include, without limitation, reducing a size of the processable portion by analyzing outliers, voxels, and normals, expanding a region from the reduced-sized processable portion, determining an initial drivable surface from the expanded region, segmenting and meshing the initial drivable surface, locating polygons within the segmented and meshed initial drivable surface, and setting the drivable surface based at least on the polygons. The fourth processor may optionally include executable code that may include, but is not limited to, sorting the drivable surface point cloud data according to an 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 categories of points, in combination, satisfy at least one first preselected criterion. The system may optionally include executable code that may include, but is not limited to, creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF point, in combination, satisfy at least one second preselected criterion.

[0009] Creating a graphed polygon may optionally include executable code that may include, but is not limited to, creating at least one convex polygon from at least one drivable surface, where the at least one convex polygon includes an edge, smoothing the edge, forming a driving margin based on the smoothed edge, adding at least one SDSF trajectory to the at least one drivable surface, and removing the edge from the at least one drivable surface according to at least one third preselected criterion. Smoothing the edge may optionally include executable code that may include, but is not limited to, trimming the edge outward. Forming a driving margin of the smoothed edge may optionally include executable code that may include, but is not limited to, trimming the outer edge inward.

[0010] In some configurations, a method of the present teachings for navigating at least one SDSF encountered by a TD includes the TD traveling a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, and the method can include, without limitation, accessing a route configuration. The route configuration can include at least one graphed polygon that can include filtered point cloud data. The point cloud data can include labeled features and a drivable margin. The method can include transforming the point cloud data to a global coordinate system, determining a boundary of the at least one SDSF, creating an SDSF buffer of a preselected size around the boundary, determining which of the at least one SDSF is traversable 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 configuration, and selecting a route from the start point to a destination point based at least on the edge / weight graph.

[0011] The at least one SDSF crossing criterion can optionally include a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF, a minimum entry distance and a minimum exit distance between the at least one SDSF and the TD that includes a drivable surface, and a minimum entry distance between the at least one SDSF and the TD that can accommodate an approximately 90° approach to the at least one SDSF by the TD.

[0012] In some configurations, a system of the present teachings for navigating at least one SDSF encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, can include, without limitation, a sixth processor accessing a route form. The route form can include at least one graphed polygon that can include filtered point cloud data. The point cloud data can include labeled features and a drivable margin. The system can include a seventh processor transforming the point cloud data to a global coordinate system and an eighth processor determining a boundary of the at least one SDSF. The eighth processor can create an SDSF buffer of a preselected size around the boundary. The system may include at least a ninth processor that determines which of the at least one SDSF is traversable based on the at least one SDSF traversal criterion; a tenth processor that creates an edge / weight graph based on at least the at least one SDSF traversal criterion, the transformed point cloud data, and the route topology; and a base controller that plans a route from the start point to the destination point based on at least the edge / weight graph.

[0013] In some configurations, a method of the present teachings for navigating at least one SDSF encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, the method can include, without limitation, accessing point cloud data representing the surface. The method can include filtering the point cloud data, forming the filtered point cloud data into processable portions, and merging the processable portions into at least one concave polygon. The method can include locating and labeling at least one SDSF within the at least one concave polygon. The locating and labeling can form labeled point cloud data. The method can include creating a graphing polygon based at least on the at least one concave polygon. The graphing polygon can form a route configuration, and the point cloud data can include labeled features and a drivable margin. The method may include transforming the point cloud data to a global coordinate system, determining a boundary of at least one SDSF, creating an SDSF buffer of a preselected size around the boundary, determining which of the at least one SDSF is traversable based on at least one SDSF traversal criterion, creating an edge / weight graph based on at least the at least one SDSF traversal criterion, the transformed point cloud data, and a route topology, and selecting a route from a start point to a destination point based on at least the edge / weight graph.

[0014] Filtering the point cloud data can optionally include conditionally removing points representing transient objects and outliers from the point cloud data and replacing the removed points with a preselected height. Forming the processing portions can optionally include dividing the point cloud data into processable portions and removing points at the preselected height from the processable portions. Merging the processable portions can optionally include reducing a size of the processable portions by analyzing outliers, voxels, and normals, expanding a region from the reduced-sized processable portions, determining an initial drivable surface from the expanded region, dividing and meshing the initial drivable surface, locating polygons within the divided and meshed initial drivable surface, and setting the drivable surface based at least on the polygons. Locating and labeling at least one SDSF can optionally include sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points, and locating the at least one SDSF point based at least on whether the categories of points, in combination, satisfy at least one first preselected criterion. The method optionally can include creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF point, in combination, satisfies at least one second preselected criterion. Creating a graphing polygon can further optionally include creating at least one convex polygon from the at least one drivable surface. The at least one convex polygon can include an edge. Creating a graphing polygon can include smoothing the edge, forming a driving margin based on the smoothed edge, adding the at least one SDSF trajectory to the at least one drivable surface, and removing the edge from the at least one drivable surface according to at least one third preselected criterion. Smoothing the edges can optionally include trimming the edges outward.Forming the smoothed edge running margin can optionally include trimming the outer edge inward. The at least one SDSF crossing criterion can optionally include a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF, a minimum entry distance and a minimum exit distance between the at least one SDSF and the TD that includes a drivable surface, and a minimum entry distance between the at least one SDSF and the TD that can accommodate an approximately 90° approach by the TD to the at least one SDSF.

[0015] In some configurations, a system of the present teachings for navigating at least one SDSF encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, can include, but is not limited to, a point cloud accessor that accesses point cloud data representing the surface, a first filter that filters the point cloud data, a segmenter that forms processable portions from the filtered point cloud data, a third processor that merges the processable portions into at least one concave polygon, a fourth processor that locates and labels the at least one SDSF within the at least one concave polygon, where the locating and labeling forms labeled point cloud data, and a fifth processor that creates a graphing polygon. The route configuration can include at least one graphing polygon that can include the filtered point cloud data. The point cloud data can include labeled features and a drivable margin. The system may include a seventh processor that transforms the point cloud data to a global coordinate system and an eighth processor that determines a boundary of the at least one SDSF. The eighth processor may create an SDSF buffer of a preselected size around the boundary. The system may include a ninth processor that determines which of the at least one SDSF is traversable based on at least one SDSF traversal criterion, a tenth processor that creates an edge / weight graph based on at least the at least one SDSF traversal criterion, the transformed point cloud data, and a route topology, and a base controller that plans a route from a start point to a destination point based on at least the edge / weight graph.

[0016] The first filter may optionally include executable code that may include, without limitation, conditionally removing points representing transient objects and points representing outliers from the point cloud data and replacing the removed points with a preselected height. The segmenter may optionally include executable code that may include, without limitation, dividing the point cloud data into processable portions and removing points at a preselected height from the processable portions. The third processor may optionally include executable code that may include, without limitation, reducing a size of the processable portion by analyzing outliers, voxels, and normals, expanding a region from the reduced-sized processable portion, determining an initial drivable surface from the expanded region, segmenting and meshing the initial drivable surface, locating polygons within the segmented and meshed initial drivable surface, and setting the drivable surface based at least on the polygons. The fourth processor may optionally include executable code that may include, but is not limited to, sorting the drivable surface point cloud data according to an 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 categories of points, in combination, satisfy at least one first preselected criterion. The system may optionally include executable code that may include, but is not limited to, creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF point, in combination, satisfy at least one second preselected criterion.

[0017] Creating a graphed polygon may optionally include executable code that may include, but is not limited to, creating at least one convex polygon from at least one drivable surface, where the at least one convex polygon includes an edge; smoothing the edge; forming a driving margin based on the smoothed edge; adding at least one SDSF trajectory to the at least one drivable surface; and removing the edge from the at least one drivable surface according to at least one third preselected criterion. Smoothing the edge may optionally include executable code that may include, but is not limited to, trimming the edge outward. Forming a driving margin of the smoothed edge may optionally include executable code that may include, but is not limited to, trimming the outer edge inward.

[0018] In some configurations, a method of the present teachings for navigating a transport device (TD) along a path line within a travel area toward a target point that traverses at least one SDSF, the TD including a leading edge and a trailing edge, can include, but is not limited to, receiving SDSF information and obstacle information related to the travel area, detecting at least one candidate SDSF from the SDSF information, and selecting an SDSF line from the at least one candidate SDSF line based on at least one selection criterion. The method can include determining at least one traversable portion of the selected SDSF line based on at least one location of at least one obstacle found in the obstacle information near the selected SDSF line, directing the TD toward the at least one traversable portion by turning the TD to travel along a line perpendicular to the traversable portion and operating the TD at a first speed, and constantly correcting the travel direction of the TD based on a relationship between the travel direction and the perpendicular line. The method can include traveling the TD at a second speed by adjusting the first speed of the TD based at least on the travel direction and the distance between the TD and the traversable portion. If the SDSF associated with at least one traversable portion is high relative to the surface of the travel route, the method may include crossing the SDSF by raising the leading edge relative to the trailing edge and running the TD at a third increased speed according to the degree of elevation, and running the TD at a fourth speed until the TD clears the SDSF.

[0019] Detecting at least one candidate SDSF from the SDSF information can optionally include (a) drawing a closed polygon encompassing the location of the TD and the location of the target point, (b) drawing a path line between the location of the target point and the TD, (c) selecting two SDSF points from the SDSF information, the SDSF points being located within the polygon, and (d) drawing an SDSF line between the two points. Detecting at least one candidate SDSF can include (e) repeating steps (c)-(e) if there are fewer than a first preselected number of points within a first preselected distance of the SDSF line, and if there are fewer than a second preselected number of attempts in selecting SDSF points and drawing a line between them around the SDSF line. Detecting at least one candidate SDSF may include (f) fitting a curve to the SDSF points that fall within a first preselected distance of the SDSF line if there are more than a first preselected number of points, and (g) identifying the curve as an SDSF line if a first number of SDSF points within the first preselected distance of the curve exceeds a second number of SDSF points within the first preselected distance of the SDSF line, and if the curve intersects the path line, and if there are no gaps between SDSF points on the curve that exceed a second preselected distance. Detecting at least one candidate SDSF may include (h) repeating steps (f)-(h) if the number of points within the first preselected distance of the curve does not exceed the number of points within the first preselected distance of the SDSF line, or if the curve does not intersect the path line, or if there are gaps between SDSF points on the curve that exceed a second preselected distance, and if the SDSF line does not remain stable, and if steps (f)-(h) have not been attempted more than a second preselected number of times.

[0020] The closed polygon can optionally include a preselected width, which can optionally include a width dimension of the TD. Selecting the SDSF points can optionally include random selection. The at least one selection criterion can optionally include a first number of SDSF points within a first preselected distance of the curve exceeding a second number of SDSF points within a first preselected distance of the SDSF line, the curve intersecting the path line, and no gaps between SDSF points on the curve exceeding a second preselected distance.

[0021] Determining at least one traversable portion of the selected SDSF can optionally include selecting a plurality of obstacle points from the obstacle information. Each of the plurality of obstacle points can include a probability that the obstacle point is associated with at least one obstacle. Determining the at least one traversable portion can include projecting the plurality of obstacle points onto the SDSF line to form at least one projection if the probability is higher than a preselected percentage, any of the plurality of obstacle points is located between the SDSF line and the target point, and any of the plurality of obstacle points is closer than a third preselected distance from the SDSF line. Determining the at least one traversable portion can optionally include connecting at least two of the at least one projection to each other, locating endpoints of the at least two connected projections along the SDSF line, marking the at least two connected projections as non-traversable SDSF segments, and marking an SDSF line outside the non-traversable segment as at least one traversable segment.

[0022] Traversing at least one traversable portion of the SDSF can optionally include turning the TD to travel along a line perpendicular to the traversable portion, pointing the TD toward the traversable portion, and operating the TD at a first speed, constantly correcting the direction of travel of the TD based on a relationship between the direction of travel and the perpendicular line, and traveling the TD at a second speed by adjusting the first speed of the TD based on at least the direction of travel and the distance between the TD and the traversable portion. Traversing at least one traversable portion of the SDSF can optionally include, if the SDSF is high relative to a surface of the travel route, crossing the SDSF by raising the leading edge relative to the trailing edge and traveling the TD at a third increased speed according to the degree of elevation, and traveling the TD at a fourth speed until the TD passes the SDSF.

[0023] Traversing at least one traversable portion of the SDSF may alternatively and optionally include: (a) ignoring updates to the SDSF information and running the TD at a preselected speed if the heading error is less than a third preselected amount relative to a line perpendicular to the SDSF line; (b) running the TD forward and increasing the speed of the TD to an eighth preselected speed according to the degree of rise if the elevation of the front portion of the TD relative to the rear portion of the TD is between a sixth preselected amount and a fifth preselected amount; (c) running the TD forward at a seventh preselected speed if the front portion is elevated less than the sixth preselected amount relative to the rear portion; and (d) repeating steps (a)-(d) if the rear portion is less than or equal to a fifth preselected distance from the SDSF line.

[0024] In some configurations, the wheels of the SDSF and TD can be automatically aligned to avoid system instability. Automatic alignment can be implemented, for example, but not limited to, by continuously testing and correcting the direction of travel of the TD as it approaches the SDSF. Another aspect of the SDSF traversal feature of the present teachings is that it automatically verifies that sufficient free space exists around the SDSF before attempting to traverse. Yet another aspect of the SDSF traversal feature of the present teachings is its ability to traverse SDSFs of various geometries. Geometric shapes can include, for example, but not limited to, square and contoured SDSFs. The orientation of the TD relative to the SDSF can determine the speed and direction the TD travels. The SDSF traversal feature can adjust the speed of the TD in the vicinity of the SDSF. When the TD ascends to the SDSF, speed can be increased to assist the TD in traversing the SDSF. The present invention provides, for example, the following. (Item 1) 1. A method of navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The method comprises: accessing point cloud data representing the surface; filtering the point cloud data; forming the filtered point cloud data into a processable portion; merging the processable portions into at least one concave polygon; locating and labeling the at least one SDSF within the at least one concave polygon, wherein the locating and labeling forms labeled point cloud data; and generating a graphing polygon based at least on said at least one concave polygon; determining the path from the start point to the end point based at least on the graphed polygon; Including, The TD traverses the at least one SDSF along the path. (Item 2) filtering the point cloud data conditionally removing points representing transient objects and points representing outliers from the point cloud data; replacing the removed points with a preselected height; and Item 1. The method according to item 1, comprising: (Item 3) Forming the processing portion comprises: Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; Item 1. The method according to item 1, comprising: (Item 4) Merging the processable portions comprises: reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; Item 1. The method according to item 1, comprising: (Item 5) Locating and labeling the at least one SDSF comprises: sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; identifying the location of at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion; Item 5. The method according to item 4, comprising: (Item 6) 6. The method of claim 5, further comprising generating at least one SDSF trajectory based on whether a plurality of the at least one SDSF points, in combination, satisfy at least one second preselected criterion. (Item 7) generating the graphed polygon, creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion; and Item 7. The method of item 6, further comprising: (Item 8) 9. The method of claim 8, wherein smoothing the outer edge comprises trimming the outer edge outward to form an outer edge. (Item 9) 8. The method of claim 7, wherein forming the running margin of the smoothed outer edge comprises trimming the outer edge inward. (Item 10) 1. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The system comprises: A sensor, A map processor; a power base; Device Controller and Equipped with The device controller a first processor that accesses point cloud data representing the surface; a filter for filtering the point cloud data; a second processor for forming a processable portion from the filtered point cloud data; a third processor for merging the processable portions into at least one concave polygon; a fourth processor for locating and labeling the at least one SDSF within the at least one concave polygon, the locating and labeling forming labeled point cloud data; a fifth processor for creating graphing polygons; a sixth processor for determining the path from the start point to the end point based on at least the graphed polygon; Including, The TD traverses the at least one SDSF along the path. The filter comprises a seventh processor, the seventh processor comprising: conditionally removing points representing transient objects and points representing outliers from the point cloud data; replacing the removed points with a preselected height; and Item 11. The system of item 10, which executes code including: (Item 12) The second processor Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; Item 11. The system of item 10, comprising executable code comprising: (Item 13) The third processor reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; Item 11. The system of item 10, comprising executable code comprising: (Item 14) The fourth processor sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; identifying the location of at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion; Item 14. The system of item 13, comprising executable code comprising: (Item 15) Item 15. The system of item 14, wherein the fourth processor includes executable code that includes at least generating at least one SDSF trajectory based on whether a plurality of the at least one SDSF point, in combination, satisfies at least one second preselected criterion. (Item 16) Creating a graphing polygon is creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion; and Item 14. The system of item 13, further comprising an eighth processor including executable code comprising: (Item 17) Item 17. The system of item 16, wherein a ninth processor includes executable code for smoothing the outer edge including trimming the outer edge outward to form an outer edge. (Item 18) Item 18. The system of item 17, wherein a tenth processor includes executable code for forming the running margin of the smoothed outer edge including trimming the outer edge inward. (Item 19) 1. A method of navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The method comprises: accessing a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; transforming the point cloud data into a global coordinate system; determining a boundary of the at least one substantially discontinuous surface feature (SDSF); creating an SDSF buffer of a preselected size around said boundary; determining which of the at least one SDSF is traversable based on at least one SDSF traversal criterion; generating an edge / weight graph based on at least the at least one SDSF crossing criterion, the transformed point cloud data, and the route morphology; selecting the path from the start point to the end point based at least on the edge / weight graph; A method comprising: (Item 20) The at least one SDSF cross-sectional criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; a minimum entry distance and a minimum exit distance between the at least one SDSF including a drivable surface and the TD; Equipped with 20. The method of claim 19, wherein the minimum approach distance between the at least one SDSF and the TD allows for an approximately 90° approach to the at least one SDSF by the TD. (Item 21) 1. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD navigates a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, the system comprising: a first processor that accesses a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; a second processor for transforming the point cloud data into a global coordinate system; a third processor that determines a boundary of the at least one SDSF, the third processor creating an SDSF buffer of a preselected size around the boundary; a fourth processor that determines which of the at least one SDSF are traversable based on at least one SDSF traversal criterion; a fifth processor that creates an edge / weight graph based on at least the at least one SDSF crossing criterion, the transformed point cloud data, and the root morphology; a base controller that selects the path from the start point to the end point based at least on the edge / weight graph; A system comprising: (Item 22) The at least one SDSF cross-sectional criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; a minimum entry distance and a minimum exit distance between the at least one SDSF including a drivable surface and the TD; Equipped with 22. The system of claim 21, wherein the minimum approach distance between the at least one SDSF and the TD allows for an approximately 90° approach by the TD to the at least one SDSF. (Item 23) 1. A method of navigating at least one substantially discontinuous surface feature (SDSF) encountered by a TD, the TD navigating a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The method comprises: accessing point cloud data representing the surface; filtering the point cloud data; forming the filtered point cloud data into a processable portion; merging the processable portions into at least one concave polygon; locating and labeling the at least one SDSF within the at least one concave polygon, wherein the locating and labeling forms labeled point cloud data; and creating a graphing polygon based at least on the at least one concave polygon, the graphing polygon forming a root form; transforming the point cloud data into a global coordinate system; determining a boundary of said at least one SDSF; creating an SDSF buffer of a preselected size around said boundary; determining which of the at least one SDSF is traversable based on at least one SDSF traversal criterion; generating an edge / weight graph based on at least the at least one SDSF crossing criterion, the transformed point cloud data, and the route morphology; selecting the path from the start point to the end point based at least on the edge / weight graph; A method comprising: (Item 24) filtering the point cloud data conditionally removing points representing transient objects and points representing outliers from the point cloud data; replacing the removed points with a preselected height; and Item 24. The method according to Item 23, comprising: (Item 25) Forming the processing portion comprises: Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; Item 24. The method according to Item 23, comprising: (Item 26) Merging the processable portions comprises: reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; Item 24. The method according to Item 23, comprising: (Item 27) Locating and labeling the at least one SDSF comprises: sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; identifying the location of at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion; Item 27. The method according to Item 26, comprising: (Item 28) 28. The method of claim 27, further comprising generating at least one SDSF trajectory based on whether a plurality of the at least one SDSF points, in combination, satisfy at least one second preselected criterion. (Item 29) Creating a graphing polygon is creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion; and 29. The method of claim 28, further comprising: (Item 30) 30. The method of claim 29, wherein smoothing the outer edge comprises trimming the outer edge outward to form an outer edge. (Item 31) Item 31. The method of item 30, wherein forming the running margin of the smoothed outer edge includes trimming the outer edge inward. (Item 32) The at least one SDSF cross-sectional criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; a minimum entry distance and a minimum exit distance between the at least one SDSF including a drivable surface and the TD; Equipped with 24. The method of claim 23, wherein the minimum approach distance between the at least one SDSF and the TD allows for an approximately 90° approach to the at least one SDSF by the TD. (Item 33) 1. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The system comprises: a first processor that accesses point cloud data representing the surface; a first filter for filtering the point cloud data; a second processor for forming a processable portion from the filtered point cloud data; a third processor for merging the processable portions into at least one concave polygon; a fourth processor for locating and labeling the at least one SDSF within the at least one concave polygon, the locating and labeling forming labeled point cloud data; a fifth processor for creating graphing polygons; a sixth processor accessing a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; a seventh processor that converts the point cloud data into a global coordinate system; an eighth processor that determines a boundary of the at least one SDSF, the eighth processor creating an SDSF buffer of a preselected size around the boundary; a ninth processor that determines which of the at least one SDSF are traversable based on at least one SDSF traversal criterion; a tenth processor that creates an edge / weight graph based on at least the at least one SDSF crossing criterion, the transformed point cloud data, and the root morphology; a base controller that selects the path from the start point to the end point based at least on the edge / weight graph; A system comprising: (Item 34) The first filter comprises: conditionally removing points representing transient objects and points representing outliers from the point cloud data; replacing the removed points with a preselected height; and Item 34. The system of item 33, comprising executable code comprising: (Item 35) The second processor Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; Item 34. The system of item 33, comprising executable code comprising: (Item 36) The third processor reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; Item 34. The system of item 33, comprising executable code comprising: (Item 37) The fourth processor sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; identifying the location of at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion; Item 37. The system of item 36, comprising executable code comprising: (Item 38) Item 38. The system of item 37, comprising executable code that includes at least generating at least one SDSF trajectory based on whether a plurality of the at least one SDSF point, in combination, meets at least one second preselected criterion. (Item 39) Creating a graphing polygon is creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion; and Item 39. The system of item 38, comprising executable code comprising: (Item 40) 40. The system of claim 39, wherein smoothing the outer edge comprises executable code that includes trimming the outer edge outward to form an outer edge. (Item 41) Item 41. The system of item 40, wherein forming the running margin of the smoothed outer edge includes executable code that includes trimming the outer edge inward. (Item 42) The at least one SDSF cross-sectional criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; a minimum entry distance and a minimum exit distance between the at least one SDSF including a drivable surface and the TD; Equipped with 34. The system of claim 33, wherein the minimum approach distance between the at least one SDSF and the TD allows for an approximately 90° approach by the TD to the at least one SDSF. (Item 43) 1. A method of navigating a transport device (TD) along a path line within a travel area toward a target point that traverses at least one substantially discontinuous surface feature (SDSF), the TD including a leading edge and a trailing edge, and the SDSF including an SDSF point; The method comprises: receiving SDSF information and obstacle information regarding the travel area; detecting at least one candidate SDSF from the SDSF information; selecting an SDSF line from the at least one candidate SDSF based on at least one selection criterion; determining at least one traversable portion of the selected SDSF line based on at least one location of at least one obstacle found in the obstacle information near the selected SDSF line; directing the TD toward the at least one traversable portion by redirecting the TD to travel along a path of travel perpendicular to the at least one traversable portion and operating at a first speed; constantly correcting the direction of travel of the TD based on the relationship between the direction of travel and the travel path; traveling the TD at a second speed by adjusting a first speed of the TD based at least on the direction of travel and a distance between the TD and the at least one traversable portion; if an SDSF associated with said at least one traversable portion is high relative to a surface of said travel area, crossing said SDSF by raising said leading edge relative to said trailing edge and running said TD at a third increased speed according to the degree of elevation; running the TD at a fourth speed until the TD passes the SDSF; A method comprising: (Item 44) Detecting at least one candidate SDSF from the SDSF information includes: (a) drawing a closed polygon encompassing the TD location of the TD and the target point location of the target point; (b) drawing a straight line between the target point location and the TD location; (c) selecting two of the SDSF points from the SDSF information, the two SDSF points being located within the closed polygon; and (d) drawing an SDSF line between said two SDSF points; (e) repeating steps (c)-(e) if there are fewer than a first preselected number of points within the first preselected distance of the SDSF line and if fewer than a second preselected number of attempts have been made in selecting the SDSF points; (f) fitting a curve to the SDSF points that fall within the first preselected distance of the SDSF line if there are more than the first preselected number of points; (g) identifying the curve as the SDSF line if a first number of the SDSF points within the first preselected distance of the curve exceeds a second number of the SDSF points within the first preselected distance of the SDSF line, if the curve intersects the straight line, and if there are no gaps between the SDSF points on the curve that exceed a second preselected distance; (h) repeating steps (f)-(h) if a first number of points within the first preselected distance of the curve does not exceed a second number of points within the first preselected distance of the SDSF line, or if the curve does not intersect the straight line, or if there are gaps between the SDSF points on the curve that exceed the second preselected distance, and if the SDSF line does not remain stable, and if steps (f)-(h) have not been attempted more than the second preselected number of times. Item 44. The method according to Item 43, comprising: (Item 45) Item 46. The method of item 44, wherein the closed polygon can include a preselected width. Item 46. The method of item 45, wherein the preselected width comprises a width dimension of the TD. (Item 47) 45. The method of claim 44, wherein selecting the SDSF points comprises random selection. (Item 48) The at least one selection criterion is: a first number of the SDSF points within the first preselected distance of the curve exceeds a second number of SDSF points within the first preselected distance of the SDSF line; the curve intersects with the straight line; the gap between said SDSF points on said curve does not exceed a second preselected distance; Item 45. The method of item 44, comprising: (Item 49) Determining at least one traversable portion of the selected SDSF comprises: selecting a plurality of obstacle points from the obstacle information, each of the plurality of obstacle points including a probability that each of the plurality of obstacle points is associated with the at least one obstacle; if the probability is higher than a preselected percentage and any of the plurality of obstacle points is located between the SDSF line and the target point, and if any of the plurality of obstacle points is closer than a third preselected distance from the SDSF line, projecting the plurality of obstacle points onto the SDSF line to form at least one projection; connecting at least two of the at least one projection with each other; locating endpoints of the at least two connected projections along the SDSF line; marking the at least two connected projections as non-traversable segments; marking the SDSF line outside the non-traversable segment as at least one traversable segment; Item 44. The method according to Item 43, comprising: (Item 50) 1. A method for locating features from camera images received by a robot having a pose, the method comprising: receiving the camera images from a sensor mounted on the robot, each of the camera images including an image timestamp, each of the camera images having image color pixels and image depth pixels; receiving the pose of the robot, the pose having a pose timestamp; determining a selected camera image by identifying one of the camera images having an image timestamp closest to the pose timestamp; separating the image color pixels from the image depth pixels in the selected camera image; determining an image surface classification for the selected camera image by providing the image color pixels to a first machine learning model and providing the image depth pixels to a second machine learning model; determining perimeter points of a feature in the selected camera image, the feature including feature pixels within a perimeter defined by the perimeter points, each of the feature pixels having the same surface classification, and each of the perimeter points having a set of coordinates; converting each of said sets of coordinates to UTM coordinates; A method comprising: (Item 51) 1. A method for including at least one substantially discontinuous surface feature (SDSF) in a map, the map being used by a transport device, the transport device traversing the at least one SDSF, the method comprising: removing transient data from the point cloud dataset, said removing forming processed point cloud data; Dividing the processed point cloud data into sections having a preselected number of points; removing points from the segmented processed point cloud data, the removed points having preselected height values, said removing forming segmented point cloud data; generating a concave polygon having a preselected size from the segmented point cloud data; organizing points within said concave polygon into a preselected number of categories; applying first preselected criteria to the categories to form filtered categories; averaging the filtered categories; and connecting the averaged filtered categories together to form the at least one SDSF; combining the concave polygon with the at least one SDSF to form a merged polygon; filtering the merged polygons according to a second preselected criterion; providing the map to the transport device; A method comprising: (Item 52) Item 52. The method of item 51, further comprising categorizing points within the concave polygon as upper donut points, lower donut points, and cylinder points. [Brief explanation of the drawings]

[0025] The present teachings may be more readily understood by reference to the following description taken in conjunction with the accompanying drawings, in which:

[0026] [Figure 1] FIG. 1 is a schematic block diagram of a system of the present teachings for preparing a travel path for a TD.

[0027] [Figure 2] FIG. 2 is a pictorial representation of an exemplary configuration of a device incorporating a system of the present teachings.

[0028] [Figure 3] FIG. 3 is a schematic block diagram of a map processor of the present teachings.

[0029] [Figure 4] FIG. 4 is a pictorial representation of the first part of the map processor flow of the present teachings.

[0030] [Figure 5] FIG. 5 is an image of a segmented point cloud of the present teachings.

[0031] [Figure 6] FIG. 6 is a pictorial representation of a second portion of the map processor of the present teachings.

[0032] [Figure 7] FIG. 7 is an image of the drivable surface detection results of the present teachings.

[0033] [Figure 8] FIG. 8 is a pictorial representation of the SDSF detector flow of the present teachings.

[0034] [Figure 9] FIG. 9 is a pictorial representation of the SDSF categories of the present teachings.

[0035] [Figure 10]FIG. 10 is an image of an SDSF identified by the system of the present teachings.

[0036] [Figure 11A] 11A and 11B are pictorial representations of the polygon processing of the present teachings. [Figure 11B] 11A and 11B are pictorial representations of the polygon processing of the present teachings.

[0037] [Figure 12] FIG. 12 is an image of the polygon and SDSF identified by the system of the present teachings.

[0038] [Figure 13] FIG. 13 is a schematic block diagram of a device controller of the present teachings.

[0039] [Figure 14] FIG. 14 is a schematic block diagram of an SDSF processor of the present teachings.

[0040] [Figure 15] FIG. 15 is an image of the SDSF approach identified by the system of the present teachings.

[0041] [Figure 16] FIG. 16 is an image of a route configuration produced by the system of the present teachings.

[0042] [Figure 17] FIG. 17 is a schematic block diagram of a mode of the present teachings.

[0043] [Figure 18A] 18A-18E are flowcharts of methods of the present teachings for traversing an SDSF. [Figure 18B] 18A-18E are flowcharts of methods of the present teachings for traversing an SDSF. [Figure 18C]18A-18E are flowcharts of methods of the present teachings for traversing an SDSF. [Figure 18D] 18A-18E are flowcharts of methods of the present teachings for traversing an SDSF. [Figure 18E] 18A-18E are flowcharts of methods of the present teachings for traversing an SDSF.

[0044] [Figure 19] FIG. 19 is a schematic block diagram of a system of the present teachings for traversing an SDSF.

[0045] [Figure 20A] 20A-20C are pictorial representations of the method of FIGS. 18A-18C. [Figure 20B] 20A-20C are pictorial representations of the method of FIGS. 18A-18C. [Figure 20C] 20A-20C are pictorial representations of the method of FIGS. 18A-18C.

[0046] [Figure 21] Figure 21 is a pictorial representation of converting an image into a polygon. DETAILED DESCRIPTION OF THE INVENTION

[0047] The SDSF traversal feature of the present teachings can utilize a TD, for example, but not limited to, an autonomous or semi-autonomous device, to navigate within an environment that may include features such as an SDSF. The SDSF traversal feature can enable the TD to navigate over an extended variety of surfaces. In particular, the SDSF can be precisely identified and labeled so that the TD can automatically maintain TD performance during SDSF entry and exit, and TD speed, mode, and direction can be controlled for safe SDSF traversal.

[0048] Referring now to FIG. 1 , a system 100 for managing traversal of an SDSF may include a TD 101, a core cloud infrastructure 103, a TD service 105, a device controller 111, sensors 701, and a power base 112. The TD 101 may transport goods and / or people from an origin to a destination along a route that is dynamically determined as modified by incoming sensor information, for example, but not limited to, the TD 101. The TD 101 may include, but is not limited to, devices with autonomous modes, devices that can operate fully autonomously, devices that can be operated at least partially remotely, and devices that may include a combination of these features. The TD service 105 may provide drivable surface information, including the features, to the device controller 111. The device controller 111 may modify the drivable surface information according to at least, for example, but not limited to, the incoming sensor information and feature traversal requirements, and may plan a route for the TD 101 based on the modified drivable surface information. The device controller 111 can submit commands to the power base 112, which instructs the power base 112 to provide speed, direction, and vertical movement commands to the wheel motors and cluster motors, causing the TD 101 to follow a selected route and raise and lower its cargo accordingly. The TD services 105 can access route-related information from the core cloud infrastructure 103, which may include, but is not limited to, storage and content distribution facilities. In some configurations, the core cloud infrastructure 103 may be hosted by, for example, but not limited to, AMAZON WEB SERVICES®, GOOGLE CLOUD TM , and commercial products such as ORACLE CLOUD®.

[0049] Referring now to FIG. 2, an exemplary TD that may include the device controller 111 (FIG. 1) and map processor 104 (FIG. 1) of the present teachings is disclosed, for example, but not limited to, U.S. patent application Ser. No. 16 / 035,205, filed July 13, 2018, entitled "Mobility Device," or U.S. patent application Ser. No. 16 / 035,205, filed August 15, 2001, entitled "Control No. 6,571,892, entitled "Power Base System and Method," incorporated herein by reference in its entirety. An exemplary power base assembly is described herein not to limit the present teachings, but instead to highlight features of any power base assembly that may be useful in implementing the techniques of the present teachings. The exemplary power base assembly may optionally include a power base 112, a wheel cluster assembly 21100, and a payload carrier height assembly 30068. The exemplary power base assembly may optionally provide electrical and mechanical power to drive the wheels 21203 and the cluster 21100, which may raise and lower the wheels 21203. The power base 112 may control the rotation of the cluster assembly 21100 and the elevation and lowering of the payload carrier height assembly 30068 to support the substantially discontinuous surface traversal of the present teachings. Other such devices can also be used to adapt the SDSF detection and traversal of the present teachings.

[0050] Continuing with reference to FIG. 1 , in some configurations, sensors internal to the exemplary power base can detect the orientation and rate of change of the TD101, motors can enable servo operation, and a controller can interpret information from the internal sensors and motors. Appropriate motor commands can be calculated to achieve transport performance and implement path-following commands. Left and right wheel motors can drive wheels on both sides of the TD101 ( FIG. 1 ). In some configurations, the front and rear wheels can be coupled to drive together, such that the two left wheels can drive together and the two right wheels can drive together. In some configurations, turning can be accomplished by driving the left and right motors at different rates, and a cluster motor can rotate the wheelbase in the forward / rearward direction. This can allow the TD101 to remain level while the front wheels are higher or lower than the rear wheels. This feature can be useful, for example, but not limited to, when ascending or descending an SDSF. The payload carrier 173 can be automatically raised and lowered based at least on the underlying terrain.

[0051] Continuing with FIG. 1 , in some configurations, the point cloud data may include route information regarding the area through which the TD 101 should travel. Possibly collected by a mapping device similar to or the same as the TD 101, the point cloud data may be time-tagged. The path along which the mapping device travels may be referred to as a mapped trajectory. The point cloud data processing described herein may occur as the mapping device traverses the mapped trajectory or after point cloud data collection is complete. After the point cloud data are collected, they may undergo point cloud data processing, which may include initial filtering and point reduction, point cloud segmentation, and feature detection as described herein. In some configurations, the core cloud infrastructure 103 may provide long-term or short-term storage for the collected point cloud data and may provide the data to the TD service 105. The TD service 105 may select from among the possible point cloud datasets to find one that covers an area surrounding the desired starting point for the TD 101 and the desired destination for the TD 101. The TD service 105 may include, but is not limited to, a map processor 104 that may reduce the size of the point cloud data and determine features represented within the point cloud data. In some configurations, the map processor 104 may determine the location of an SDSF from the point cloud data. In some configurations, polygons may be created from the point cloud data as a technique for dividing the point cloud data and ultimately establishing a drivable surface. In some configurations, SDSF detection and drivable surface determination may proceed in parallel. In some configurations, SDSF detection and drivable surface determination may proceed sequentially.

[0052] Referring now to FIG. 3 , in some configurations, the map processor 104 can include feature extraction, which may include, but is not limited to, line-of-sight filtering 121 of the point cloud data 131 and the mapped trajectory 133. Line-of-sight filtering can remove points that are hidden from the direct line-of-sight of the sensors that collect the point cloud data and form the mapped trajectory. The reduced point cloud data 132 can be further processed by organizing 151 the reduced point cloud data 132, possibly according to preselected criteria associated with specific features. In some configurations, the organized point cloud data and the mapped trajectory 133 can be further processed by removing 153 transient points by any number of methods, including those described herein. Transient points can complicate processing, especially if the specific features are stationary. The processed point cloud data 135 can be divided into processable chunks. In some configurations, the processed point cloud data 135 can be divided (155) into segments having a preselected minimum number of points, for example, but not limited to, approximately 100,000 points. In some configurations, further point reduction is based on preselected criteria that may be related to the features to be extracted. For example, if points above a certain height are not important for locating the feature, those points can be deleted from the point cloud data. In some configurations, the height of at least one of the sensors collecting the point cloud data can be considered an origin, and points above the origin can be removed from the point cloud data, for example, because only points of interest are associated with surface features. After the filtered point cloud data 135 is divided, forming segments 137, the remaining points can be divided into drivable surface segments and surface features can be located. In some configurations, locating the drivable surface can include, for example, but not limited to, generating (161) polygons 139 as described herein. In some configurations, locating the surface features may include generating 163 an SDSF line 141 as described herein, for example, but not limited to,In some configurations, creating a dataset that can be further processed to generate an actual path along which the TD 101 (FIG. 1) may travel can include combining (165) the polygon 139 and the SDSF 141.

[0053] Referring now primarily to FIG. 4 , eliminating 153 ( FIG. 3 ) objects that are transient with respect to the mapped trajectory 133, such as exemplary time-stamped point 751, from the point cloud data 131 ( FIG. 3 ) may include casting a ray 753 from a time-stamped point on the mapped trajectory 133 to each time-stamped point in the point cloud data 131 ( FIG. 3 ) that has substantially the same time stamp. If the ray 753 intersects a point between the time-stamped point on the mapped trajectory 133 and the end point of the ray 753, e.g., point D755, the intersection point D755 may be assumed to have entered the point cloud data during a different sweep of the camera. The intersection point, e.g., intersection point D755, may be assumed to be part of a transient object and may be removed from the reduced point cloud data 132 ( FIG. 3 ) as not representing a fixed feature, such as an SDSF. The result is, for example, but not limited to, processed point cloud data 135 ( FIG. 3 ) that is free of transient objects. Points that are removed as part of a transient object, but that are also substantially at ground level, can be returned 754 to the processed point cloud data 135 (FIG. 3). Transient objects may not include certain features, such as, but not limited to, SDSF 141 (FIG. 3), and therefore can be removed without interfering with the integrity of the point cloud data 131 (FIG. 3) when SDSF 141 (FIG. 3) is the feature being detected.

[0054] Continuing with reference to FIG. 4, dividing 155 (FIG. 3) the processed point cloud data 135 (FIG. 3) into sections 757 can produce sections 757 having a preselected size and shape, for example, but not limited to, squares 154 (FIG. 5) having a minimum preselected side length and containing approximately 100,000 points. From each section 757, points not necessarily relevant to the specific task, for example, but not limited to, points located above the preselected points, can be removed 157 (FIG. 3) to reduce the dataset size. In some configurations, the preselected points can be the height of the TD 101 (FIG. 1). Removing these points can lead to more efficient processing of the dataset.

[0055] Referring again primarily to FIG. 3, the map processor 104 can provide the device controller 111 with at least one data set that can be used to generate direction, speed, and altitude commands for the TD 101 (FIG. 1). The at least one data set can include points that can be connected to other points in the data set, and each of the lines connecting the points in the data set traverses the drivable surface. To determine such route points, the segmented point cloud data 137 can be divided into polygons 139, and the vertices of the polygons 139 can potentially become route points. The polygons 139 can include features such as, for example, SDSFs 141.

[0056] Referring now primarily to FIG. 6, in some configurations, point cloud data 131 (FIG. 3) can be processed by removing outliers by conventional means, such as statistical analysis techniques, for example, but not limited to, those available at the Point Cloud Library, http: / / pointclouds.org / documentation / tutorials / statistical_outlier.php. Filtering can include downsizing segments 137 (FIG. 3) by conventional means, including, but not limited to, voxelization grid approaches, such as those available at the Point Cloud Library, http: / / pointclouds.org / documentation / tutorials / voxel_grid.php. The segmented point cloud data 137 (FIG. 3) can be used to generate concave polygons 759 (161) (FIG. 3), for example, 5m x 5m polygons. The concave polygon 759 can be created, for example, but not by way of limitation, by the process described in http: / / pointclouds.org / documentation / tutorials / hull_2d.php or the process described in "A New Concave Hull Algorithm and Concaveness Measure for n-dimensional Datasets," Park et al., Journal of Information Science and Engineering 28, pp. 587-600, 2012.

[0057] Continuing primarily with reference to FIG. 6, in some configurations, creating the processed point cloud data 135 (FIG. 3) can include filtering voxels. To reduce the number of points that will undergo future processing, in some configurations, the centroid of each voxel in the dataset can be used to approximate the points within the voxel, and all points except the centroid can be excluded from the point cloud data. In some configurations, the center of the voxel can be used to approximate the points within the voxel. Other methods for reducing the size of the filtered segment 251 can also be used, such as, for example, but not limited to, random point subsampling, such that a fixed number of uniformly randomly selected points can be excluded from the filtered segment 251.

[0058] Referring again to FIG. 3 , in some configurations, creating the processed point cloud data 135 can include calculating normals from a dataset from which outliers have been removed and which has been reduced in size through voxel filtering. The normals for each point in the filtered dataset can be used for various processing possibilities, including curve reconstruction algorithms. In some configurations, estimating and filtering normals in the dataset can include obtaining an underlying surface from the dataset using a surface meshing technique and calculating normals from the surface mesh. In some configurations, estimating normals can include using approximations to infer surface normals directly from the dataset, such as, for example, but not limited to, determining normals to a fitting plane obtained by applying a total least squares method to k nearest neighbors of a point. In some configurations, the value of k can be selected based at least on empirical data. Filtering normals can include removing any normals that are more than approximately 45° from perpendicular to the xy plane. In some configurations, a filter can be used to align normals in the same direction. If a portion of the dataset represents a planar surface, redundant information contained in neighboring normals can be filtered out either by performing random subsampling or by filtering out one point from the set of associated points. In some configurations, selecting a point can include recursively decomposing the dataset into boxes until each box contains at most k points. A single normal can be calculated from the k points in each box.

[0059] Continuing with reference to FIG. 3 , in some configurations, creating the processed point cloud data 135 can include growing regions within the dataset by clustering points that geometrically fit a surface representing the dataset and refining the surface as the region grows to obtain the best approximation for the maximum number of points. Region growing can merge points in terms of a smoothness constraint. In some configurations, the smoothness constraint can be determined empirically or based on a desired surface smoothness, for example. In some configurations, the smoothness constraint can include a range from about 10π / 180 to about 20π / 180. The output of region growing is a set of point clusters, each of which is a set of points, each of which is considered to be part of the same smooth surface. In some configurations, region growing can be based on a comparison of angles between normals. Region growing can be performed by algorithms such as, but not limited to, Region Growing Segmentation http: / / pointclouds.org / documentation / tutorials / region_growing_segmentation.php and http: / / pointclouds.org / documentation / tutorials / cluster_extraction.php#cluster-extraction.

[0060] Referring now primarily to FIG. 7 , in some configurations, the processed point cloud data 135 ( FIG. 3 ) can be used to determine an initial drivable surface 265. Region growing can generate point clusters that may include points that are part of the drivable surface. In some configurations, a reference plane can be fitted to each of the point clusters to determine the initial drivable surface. In some configurations, the point clusters can be filtered according to the relationship between the orientation of the point cluster and the reference plane. For example, if the angle between the point cluster plane and the reference plane is less than, for example, but not by way of limitation, about 30°, the point cluster can be preliminarily considered to be part of the initial drivable surface. In some configurations, the point clusters can be filtered based on, for example, but not by way of limitation, size constraints. In some configurations, a point cluster that is larger in point size than about 20% of the total points in the point cloud data 131 can be considered too large, and a point cluster that is smaller in size than about 0.1% of the total points in the point cloud data 131 can be considered too small. The initial drivable surface can include the filtered versions of the point clusters. In some configurations, point clusters can be separated for further processing by any of several known methods. In some configurations, density-based spatial clustering for noisy applications (DBSCAN) can be used to separate point clusters, while in some configurations, k-means clustering can be used to separate point clusters. DBSCAN can group points that are densely clustered together and mark points that are substantially isolated or in low-density areas as outliers. To be considered dense, points must be located within a preselected distance from a candidate point. In some configurations, a scale factor for the preselected distance can be determined empirically or dynamically. In some configurations, the scale factor can be in the range of approximately 0.1 to 1.0.

[0061] Referring again primarily to FIG. 6 , the resulting point sub-clusters can be converted into concave polygons 759, for example, using meshing. Meshing can be performed by standard methods such as, for example, but not limited to, marching cubes, marching tetrahedra, surface nets, greedy meshing, and dual contouring. In some configurations, concave polygons 759 can be generated by projecting a local neighborhood of points along the point's normals and connecting unconnected points. The resulting concave polygons 759 can be based on at least the size of the neighborhood, the maximum allowable distance for the points to be considered, the maximum edge length for the polygon, the minimum and maximum angles of the polygon, and the maximum deviation the normals can have from each other. In some configurations, concave polygons 759 can be filtered according to whether concave polygons 759 would be too small for TD 101 ( FIG. 1 ) to traverse. In some configurations, a circle the size of TD 101 (FIG. 1) can be dragged around each of concave polygons 759 by known means. If the circle falls substantially within concave polygon 759, concave polygon 759, and therefore the resulting drivable surface, can accommodate TD 101 (FIG. 1). In some configurations, the area of ​​concave polygon 759 can be compared to the footprint of TD 101 (FIG. 1). The polygons can be assumed to be irregular, so the first step to determining the area of ​​concave polygon 759 is to separate concave polygon 759 into regular polygons 759A by known methods. For each regular polygon 759A, a standard area equation can be used to determine its size. The areas of each regular polygon 759A can be added together to determine the area of ​​concave polygon 759, which can be compared to the footprint of TD 101 (FIG. 1). The filtered concave polygons may include a subset of the concave polygons that meet a size criterion. The filtered concave polygons may be used to set the final drivable surface.

[0062] Referring primarily to FIG. 8 , generating an SDSF line 163 ( FIG. 3 ) may include locating the SDSF by further filtering the concave polygon 759 ( FIG. 6 ). In some configurations, points from the point cloud data that make up the polygon may be categorized as either upper donut points 351 ( FIG. 9 ), lower donut points 353 ( FIG. 9 ), or cylinder points 355 ( FIG. 9 ). The upper donut points 351 ( FIG. 9 ) may correspond to shapes of the SDSF model 352 that are furthest from the ground. The lower donut points 353 ( FIG. 9 ) may correspond to shapes of the SDSF model 352 that are closest to the ground or at ground level. The cylinder points 355 ( FIG. 9 ) may correspond to shapes between the upper donut points 351 ( FIG. 9 ) and the lower donut points 353 ( FIG. 9 ). A combination of categories may form a donut 371. To determine whether a donut 371 forms an SDSF, certain criteria are tested. For example, in each donut 371, there must be a minimum number of upper donut points 351 (FIG. 9) and a minimum number of lower donut points 353 (FIG. 9). In some configurations, the minimum value can be selected empirically and can fall in the range of approximately 5 to 20. Each donut 371 can be divided into multiple portions, for example, two hemispheres. Another criterion for determining whether points within a donut 371 represent an SDSF is whether a majority of the points are located within opposing hemispheres of the donut 371 portions. Cylindrical points 355 (FIG. 9) can occur in either the first cylindrical region 357 (FIG. 9) or the second cylindrical region 359 (FIG. 9). Another criterion for SDSF selection is that a minimum number of points must be present within both cylindrical regions 357 / 359 (FIG. 9). In some configurations, the minimum number of points can be selected empirically and can fall in the range of 3 to 20. Another criterion for SDSF selection is that the donut 371 must contain at least two of three categories of points: upper donut points 351 (FIG. 9), lower donut points 353 (FIG. 9), and cylinder points 355 (FIG. 9).

[0063] Continuing primarily with reference to FIG. 8, in some configurations, polygons can be processed in parallel. Each category worker 362 can search its assigned polygon for SDSF points 789 ( FIG. 12 ) and assign the SDSF points 789 ( FIG. 12 ) to categories 763 ( FIG. 6 ). As the polygons are processed, the resulting point categories 763 ( FIG. 6 ) can be combined (363) to form combined category 366, and the categories can be shortened (365) to form shortened combined category 368. Shortening the SDSF points 789 ( FIG. 12 ) can include filtering the SDSF points 789 ( FIG. 12 ) with respect to their distance from the ground. The shortened combined categories 368 are averaged by searching the area around each SDSF point 766 (FIG. 6) and generating an average point 765 (FIG. 6), possibly processed in parallel by an average worker 373, and the points of the categories can form a set of averaged donuts 375. In some configurations, the radius around each SDSF point 766 (FIG. 6) can be empirically determined. In some configurations, the radius around each SDSF point 766 (FIG. 6) can include a range of 0.1 m to 1.0 m. The height change between one point on an SDSF trajectory 377 (FIG. 6) and another on the SDSF at the average point 765 (FIG. 6) can be calculated. Connecting the averaged donuts 375 together can generate the SDSF trajectory 377 (FIG. 6). When generating the SDSF trajectory 377 (FIGS. 6 and 10), if there are two candidate next points within a search radius of the starting point, the next point may be selected based on at least forming the straightest possible line between the previous line segment, the starting point, and the candidate destination point, where the candidate next point represents the smallest change in SDSF height between the previous point and the candidate next point. In some configurations, the SDSF height may be defined as the difference between the heights of the upper donut 351 (FIG. 9) and the lower donut 353 (FIG. 9).

[0064] Referring now primarily to FIG. 11A , combining concave polygons and SDSF lines 165 ( FIG. 3 ) can generate a dataset including polygon 139 ( FIG. 3 ) and SDSF 141 ( FIG. 3 ), which can be manipulated to generate a graphed polygon using the SDSF data. Manipulating concave polygons 263 can include, but is not limited to, merging concave polygons 263 to form merged polygon 771. Merging concave polygons 263 can be performed using known methods, such as, but not limited to, those found at (http: / / www.angusj.com / delphi / clipper.php). Merged polygon 771 can be dilated to smooth edges and form dilated polygon 772. Dilated polygon 772 can be deflated to provide a running margin, forming deflated polygon 774, to which SDSF trajectory 377 ( FIG. 11B ) can be added. Inward trimming (shrinkage) can ensure that there is room near the edges for TD 101 (FIG. 1) to travel by reducing the size of the drivable surface by a preselected amount based at least on the size of TD 101 (FIG. 1). Polygon expansion and contraction can be accomplished by commercially available techniques such as, for example, but not limited to, the ARCGIS® Clip command (http: / / desktop.arcgis.com / en / arcmap / 10.3 / manage-data / editing-existing-features / clipping-a-polygon-feature.htm).

[0065] Referring now primarily to FIG. 11B , the contracted polygon 774 can be partitioned into convex polygons 778, each of which can be traversed without encountering a non-drivable surface. The contracted polygon 774 can be partitioned by conventional means such as, for example, but not limited to, ear slicing, which is optimized by z-order curve hashing and extended to handle holes, twisted polygons, degeneracy, and self-intersections. Commercially available ear slicing implementations include, but are not limited to, those found at (https: / / github.com / mapbox / earcut.hpp). The SDSF trajectory 377 can include SDSF points 789 ( FIG. 15 ) that can be connected to polygon vertices 781. The vertices 781 can be considered to be possible path points that can be connected to each other to form possible travel paths for the TD 101 ( FIG. 1 ). In the dataset, the SDSF points 789 can be labeled as such. As partitioning progresses, it is possible that redundant edges, such as, for example, but not limited to, edges 777 and 779, are introduced. Removing one of edges 777 or 779 can reduce the complexity of further analysis and preserve the convex polygon mesh. In some configurations, a Hertel-Mehlhorn polygon partitioning algorithm can be used to remove edges and omit edges labeled as features. The set of convex polygons 778, including the labeled features, can undergo further simplification to reduce the number of possible path points, which can be provided to device controller 111 (FIG. 1) in the form of annotated point data 379 (FIG. 14).

[0066] Referring now primarily to FIG. 13 , annotated point data 379 ( FIG. 14 ) can be provided to the device controller 111. The annotated point data 379 ( FIG. 14 ), which can be the basis for route information that can be used to instruct the TD 101 ( FIG. 1 ) to navigate a path, can include, but is not limited to, navigable edges, mapped trajectories, such as, but not limited to, the mapped trajectories 413 / 415 ( FIG. 16 ), and labeled features, such as, but not limited to, the SDSF 377 ( FIG. 15 ). The mapped trajectories 413 / 415 ( FIG. 15 ) can include a graph of edges in the route space and initial weights assigned to portions of the route space. The edge graph can include characteristics, such as, but not limited to, directionality and capacity, and edges can be categorized according to these characteristics. The mapped trajectories 413 / 415 ( FIG. 15 ) can include cost modifiers associated with surfaces in the route space and driving modes associated with edges. Driving modes can include, but are not limited to, path following and SDSF climbing. Other modes can include, for example, but are not limited to, autonomous, mapping, and waiting for intervention modes of operation. Ultimately, a path can be selected based on at least a lower cost qualifier. Configurations that are relatively far from the mapped trajectory 413 / 415 (FIG. 15) can have higher cost qualifiers and receive less attention when forming a path. The initial weights can be adjusted while the TD 101 (FIG. 1) is operating, potentially causing path modifications. The adjusted weights can be used to adjust the edge / weight graph 381 (FIG. 14) and can be based at least on the current driving mode, the current surface, and the edge category.

[0067] 13 , the device controller 111 can include a feature processor that can perform specific tasks related to incorporating the eccentricity of any feature into the path. In some configurations, the feature processor can include, but is not limited to, the SDSF processor 118. In some configurations, the device controller 111 can include, but is not limited to, the SDSF processor 118, the sensor processor 703, the mode controller 122, and the base controller 114, each of which are described herein. The SDSF processor 118, the sensor processor 703, and the mode controller 122 can provide inputs to the base controller 114.

[0068] Continuing with reference to FIG. 13 , the base controller 114, based on inputs provided by at least the mode controller 122, the SDSF processor 118, and the sensor processor 703, can determine information that the power base 112 can use to cause the TD 101 ( FIG. 1 ) to travel on a path determined by the base controller 114 based at least on the edge / weight graph 381 ( FIG. 14 ). In some configurations, the base controller 114 can ensure that the TD 101 ( FIG. 1 ) follows a predetermined path from a start point to a destination and can modify the predetermined path based on at least external and / or internal conditions. In some configurations, external conditions can include, but are not limited to, stop signals, SDSFs, and obstacles in or near the path being traveled by the TD 101 ( FIG. 1 ). In some configurations, internal conditions can include, but are not limited to, mode transitions that reflect the response the TD 101 ( FIG. 1 ) makes to the external conditions. The device controller 111 can determine commands to send to the power base 112 based on at least the external and internal conditions. The commands may include, but are not limited to, speed and direction commands that may instruct the TD 101 (FIG. 1) to proceed in a commanded direction at a commanded speed. Other commands may include, for example, groups of commands that enable characteristic responses, such as, for example, SDSF climbing. The base controller 114 may determine the desired speed between path waypoints by conventional methods, including, but not limited to, interior point optimizer (IPOPT) large-scale nonlinear optimization (https: / / projects.coin-or.org / Ipopt). The base controller 114 may determine the desired path based at least in part on conventional techniques, such as, but not limited to, techniques based on the Dijkstra algorithm, the A* search algorithm, or the breadth-first search algorithm. The base controller 114 may form a box around the mapped trajectory 413 / 415 (FIG. 15) to define an area within which obstacle detection may be performed. The height of the payload carrier, when adjustable, may be adjusted, at least in part, based on the commanded speed.

[0069] Continuing with reference to FIG. 13 , the base controller 114 can translate speed and direction decisions into motor commands. For example, but not by way of limitation, when encountering an SDSF, such as a curb or a slope, the base controller 114 can instruct the power base 112 in SDSF climb mode to raise the payload carrier 173 ( FIG. 2 ), align the TD 101 ( FIG. 1 ) with the SDSF at approximately a 90° angle, and reduce speed to a relatively low level. When the TD 101 ( FIG. 1 ) climbs a substantially discontinuous surface, the base controller 114 can instruct the power base 112 to transition to a climb phase in which speed is increased as increased torque is required to move the TD 101 ( FIG. 1 ) up the slope. When the TD 101 ( FIG. 1 ) encounters a relatively level surface, the base controller 114 can reduce speed to remain on any flat portions of the SDSF. In the case of a downward ramp associated with a flat section, when the TD 101 ( FIG. 1 ) begins to descend a substantially discontinuous surface and both wheels are on the downward ramp, the base controller 114 can allow the speed to increase. For example, but not limited to, when an SDSF such as a slope is encountered, the slope can be identified and treated as a structure. The structural feature can include, for example, a ramp of a preselected size. The ramp can include a slope of approximately 30° and, optionally, but not limited to, on both sides of the flat area. The device controller 111 ( FIG. 13 ) can distinguish between an obstacle and a slope by comparing the angle of the perceived feature with an expected slope angle, which can be received from the sensor processor 703 ( FIG. 13 ).

[0070] Referring now primarily to FIG. 14, from a block of drivable surface formed by a mesh of polygons represented in the annotated point data 379, the SDSF processor 118 can identify the location of navigable edges that can be used to create a path for traversal by the TD 101 (FIG. 1). Within the SDSF buffer 407 (FIG. 15), which can form an area of ​​a preselected size around the SDSF line 377 (FIG. 15), the navigable edges can be erased for special handling, assuming SDSF traversal (see FIG. 16). A closed line segment, such as segment 409 (FIG. 15), can be drawn to bisect the SDSF buffer 407 (FIG. 15) between a pair of previously determined SDSF points 789 (FIG. 12). In some configurations, for a closed line segment to be considered a candidate for SDSF traversal, the segment end 411 ( FIG. 15 ) can fall on an unobstructed portion of the drivable surface, there can be sufficient room for TD 101 ( FIG. 1 ) to travel along the line segment between adjacent SDSF points 789 ( FIG. 12 ), and the area between the SDSF points 789 ( FIG. 12 ) can be the drivable surface. The segment end 411 ( FIG. 15 ) can be connected to the underlying feature to form a vertex and a drivable edge. For example, line segments 461, 463, 465, and 467 ( FIG. 15 ), which meet the traversal criteria, are shown as part of the feature in FIG. 16 . In contrast, line segment 409 ( FIG. 15 ) did not meet the criteria because, at least, segment end 411 ( FIG. 15 ) does not fall on the drivable surface. The overlapping SDSF buffers 506 (FIG. 15) may exhibit SDSF discontinuities that may penalize the SDSF traversal of the SDSFs within the overlapped SDSF buffers 506 (FIG. 15). The SDSF lines 377 (FIG. 15) may be smoothed, and the locations of the SDSF points 789 (FIG. 12) may be adjusted so that they fall a preselected distance apart, the preselected distance being based at least on the footprint of the TD 101 (FIG. 1).

[0071] Continuing with reference to FIG. 14 , the SDSF processor 118 can convert the annotated point data 379 into an edge / weight graph 381, including morphological corrections for SDSF traversal. The SDSF processor 118 can include a seventh processor 601, an eighth processor 702, a ninth processor 603, and a tenth processor 605. The seventh processor 601 can convert coordinates of points in the annotated point data 379 into a global coordinate system to achieve compatibility with GPS coordinates and generate a GPS-compatible dataset 602. The seventh processor 601 can generate the GPS-compatible dataset 602 using conventional processes, such as, but not limited to, affine matrix transformations and PostGIS transformations. The World Geodetic System (WGS) can be used as a standard coordinate system because it takes into account the curvature of the Earth. Maps can be stored in the Universal Transverse Mercator (UTM) coordinate system and can be switched to WGS when it is necessary to find where a specific address is located.

[0072] Referring now primarily to FIG. 15, the eighth processor 702 (FIG. 14) can smooth the SDSF, determine the boundary of the SDSF 377, and create a buffer 407 around the SDSF boundary, increasing the surface cost modifier as it moves away from the SDSF boundary. The mapped trajectory 413 / 415 can be a special-case lane with the lowest cost modifier. Lower cost modifiers 406 can generally be located near the SDSF boundary, while higher cost modifiers 408 can generally be located relatively far from the SDSF boundary. The eighth processor 702 can provide the point cloud data with costs 704 (FIG. 14) to the ninth processor 603 (FIG. 14).

[0073] Continuing primarily with reference to FIG. 15, the ninth processor 603 (FIG. 14) can calculate an approximately 90° approach 604 (FIG. 14) for the TD 101 (FIG. 1) to traverse SDSFs 377 that meet the criteria for labeling them as traversable. The criteria can include SDSF width and SDSF smoothness. Line segments, such as line segment 409, can be created whose lengths indicate the minimum approach distance and minimum exit distance that the TD 101 (FIG. 1) may require to approach and exit the SDSF 377. Segment endpoints, such as endpoint 411, can be integrated with underlying routing configurations. The criteria used to determine whether an SDSF approach is possible can eliminate the possibility of some approaches. SDSF buffers, such as SDSF buffer 407, can be used to calculate valid approaches and route configuration edge creation.

[0074] Referring again primarily to FIG. 14 , the tenth processor 605 can create an edge / weight graph 381, a graph of edges and weights developed herein, from the geometry that can be used to calculate a path through the map. The geometry can include a cost modifier and a driving mode, and the edges can include directionality and capacity. The weights can be adjusted at runtime based on information from any number of sources. The tenth processor 605 can provide at least one sequence of ordered points to the base controller 114 to enable path generation, in addition to a recommended driving mode at a particular point. Each point in each sequence of points represents the location and labeling of a possible path point on the processed drivable surface. In some configurations, the labeling can indicate that the point represents a portion of a feature that may be encountered along the path, such as, for example, but not limited to, an SDSF. In some configurations, the feature can be further labeled with suggested processing based on the type of feature. For example, in some configurations, if a path point is labeled as an SDSF, further labeling can include a mode. The mode can be interpreted by TD101 (FIG. 1) as suggested driving instructions for TD101 (FIG. 1), such as switching TD101 (FIG. 1) to SDSF climb mode 100-31 (FIG. 17) to enable TD101 (FIG. 1) to traverse SDSF 377 (FIG. 15).

[0075] Referring now to FIG. 17 , in some configurations, the mode controller 122 can provide instructions to the base controller 114 ( FIG. 13 ) for executing mode transitions. The mode controller 122 can establish the mode in which the TD 101 ( FIG. 1 ) is traveling. For example, the mode controller 122 can provide a mode indication change to the base controller 114, e.g., change between a path-following mode 100-32 and an SDSF climb mode 100-31, when an SDSF is identified along the travel path. In some configurations, the annotated point data 379 ( FIG. 14 ) can include a mode identifier at various points along the route, e.g., when the mode is changed to accommodate the route. For example, if an SDSF 377 ( FIG. 15 ) is labeled in the annotated point data 379 ( FIG. 14 ), the device controller 111 can determine the mode identifier associated with the route point and potentially adjust instructions to the power base 112 ( FIG. 13 ) based on the desired mode. In addition to the SDSF climb mode 100-31 and path following mode 100-32, in some configurations, the TD 101 (FIG. 1) can support operational modes that may include, but are not limited to, a standard mode 21001, which in some configurations may involve driving two drive wheels and two swivel wheels 21001 (FIG. 2), and an enhanced mode 100-2. The enhanced mode 100-2 can provide assistance for the TD 101 (FIG. 1) to traverse uneven terrain, various environments, steep slopes, and soft terrain, as described in detail in U.S. Patent No. 6,571,892 ('892), issued June 3, 2003, entitled "Control System and Method," which is incorporated herein by reference in its entirety. In the enhanced mode 100-2, all four drive wheels 21203 (FIG. 2) can be deployed. Driving four wheels 21203 (FIG. 2) and evenly distributing weight on the wheels 21203 (FIG. 2) can enable TD101 (FIG. 1) to travel up and down steep slopes and through many types of outdoor environments, including, but not limited to, gravel, sand, snow, and mud.The height of the payload carrier 173 (FIG. 2) can be adjusted to provide the necessary clearance over obstacles and along slopes.

[0076] Referring now to FIG. 18A , a method 1150 for navigating a TD toward a target point that intersects at least one SDSF may include, but is not limited to, receiving 1151 SDSF information related to the SDSF, the target point location, and the TD location. The SDSF information may include, but is not limited to, a set of points classified as SDSF points and an associated probability for each point that the point is an SDSF point. The method 1150 may include, but is not limited to, drawing 1153 a closed polygon encompassing the TD location, the target point location, and drawing a path line between the target point and the TD location. The closed polygon may include a preselected width. Table I includes possible ranges for the preselected variables discussed herein. The method 1150 may include, but is not limited to, selecting 1155 two of the SDSF points located within the polygon and drawing 1157 an SDSF line between the two points. In some configurations, the selection of the SDSF points may be random or in any other manner. If, at 1159, there are fewer than a first preselected number of points within a first preselected distance of the SDSF line, and if, at 1161, there are fewer than a second preselected number of attempts at selecting SDSF points to draw lines between them and having fewer than the first preselected number of points around the SDSF line, method 1150 may include returning to step 1155. If, at 1161, there are a second preselected number of attempts at selecting SDSF points to draw lines between them and having fewer than the first preselected number of points around the SDSF line, method 1150 may include conceding 1163 that no SDSF line was detected.

[0077] 18B, at 1159 (FIG. 18A), if there are more than or equal to a first preselected number of points, method 1150 may include fitting a curve to points that fall within a first preselected distance of the SDSF line 1165. If, at 1167, the number of points within the first preselected distance of the curve exceeds the number of points within the first preselected distance of the SDSF line, and if, at 1171, the curve intersects with the path line, and if, at 1173, there are no gaps between points on the curve that exceed a second preselected distance, method 1150 may include identifying 1175 the curve as an SDSF line. If the number of points within a first preselected distance of the curve does not exceed the number of points within a first preselected distance of the SDSF line at 1167, or if the curve does not intersect with the path line at 1171, or if there are gaps between points on the curve that exceed a second preselected distance at 1173, and if the SDSF line does not remain stable at 1177, and if the curve fit has not been attempted more than a second preselected number of times at 1169, method 1150 may include returning to step 1165. A stable SDSF line is a result of subsequent iterations that result in the same or fewer points.

[0078] 18C , when the curve fit has been attempted for a second preselected number of attempts at 1169 (FIG. 18B), or when the SDSF line remains stable or deteriorates at 1177 (FIG. 18B), method 1150 may include receiving 1179 occupancy grid information. The occupancy grid may provide a probability that an obstacle is present at a point. The occupancy grid information may enhance the SDSF and path information found within a polygon that encompasses the TD path and SDSF when the occupancy grid includes data captured and / or calculated over a common geographic area with the polygon. Method 1150 may include selecting 1181 a point from the common geographic area and its associated probability. If the probability that an obstacle is present at the selected point is higher than a preselected percentage at 1183, and if the obstacle is located between the TD and the target point at 1185, and if the obstacle is closer than a third preselected distance from the SDSF line between the SDSF line and the target point at 1186, method 1150 may include projecting the obstacle onto the SDSF line at 1187. If the probability that the location contains an obstacle is less than or equal to the preselected percentage at 1183, or if the obstacle is not located between the TD and the target point at 1185, or if the obstacle is located at a distance equal to or greater than the third preselected distance from the SDSF line between the SDSF and the target point at 1186, and if there are more obstacles to process at 1189, method 1150 may include restarting processing at 1179.

[0079] 18D , if there are no more obstacles to process at 1189 (FIG. 18C), method 1150 may include connecting the projections and finding 1191 endpoints of the connected projections along the SDSF line. Method 1150 may include marking 1193 a portion of the SDSF line between the projection endpoints as non-traversable. Method 1150 may include marking 1195 a portion of the SDSF line outside the non-traversable segment as traversable. Method 1150 may include turning 1197 the TD to within a fifth preselected amount perpendicular to the traversable segment of the SDSF line. If, at 1199, the heading error relative to a line perpendicular to the traversable segment of the SDSF line exceeds the first preselected amount, method 1150 may include decelerating 1251 the TD by a ninth preselected amount. The method 1150 may include driving the TD forward toward the SDSF line and slowing down 1253 a second preselected amount per meter distance between the TD and the traversable SDSF line. If the distance of the TD from the traversable SDSF line is less than a fourth preselected distance at 1255, and if the heading error is greater than or equal to a third preselected amount relative to a line perpendicular to the SDSF line at 1257, the method 1150 may include slowing down 1252 the TD a ninth preselected amount.

[0080] 18E , at 1257 (FIG. 18D), if the heading error is below a third preselected amount relative to a line perpendicular to the SDSF line, method 1150 may include ignoring the updated SDSF information and driving the TD at a preselected speed 1260. If the elevation of the front portion of the TD relative to the rear portion of the TD is between a sixth preselected amount and a fifth preselected amount, at 1259, method 1150 may include driving the TD forward and increasing the speed of the TD to an eighth preselected amount according to the degree of elevation 1261. If the elevation of the front portion of the TD relative to the rear portion of the TD is below the sixth preselected amount, at 1263, method 1150 may include driving the TD forward at a seventh preselected speed 1265. If the aft end of the TD is greater than a fifth preselected distance from the SDSF line at 1267, the method 1150 can include acknowledging that the TD has completed traversing the SDSF at 1269. If the aft end of the TD is less than or equal to the fifth preselected distance from the SDSF line at 1267, the method 1150 can include returning to step 1260.

[0081] Referring now to FIG. 19 , a system 1100 for navigating a TD toward a target point that traverses at least one SDSF can include, but is not limited to, a path line processor 1103, an SDSF detector 1109, and an SDSF controller 1127. The system 1100 can be operatively coupled to a surface processor 1601, which can process sensor information, which can include, for example, but is not limited to, images of the periphery of the TD 101 ( FIG. 20A ). The surface processor 1601 can provide real-time surface feature updates, including indications of SDSFs. In some configurations, a camera can provide RGB-D data, from which points can be classified according to surface type. In some configurations, the system 1100 can process points classified as SDSFs and their associated probabilities. The system 1100 can be operatively coupled to a system controller 1602, which can manage aspects of the operation of the TD 101 ( FIG. 20A ). The system controller 1602 can maintain an occupancy grid 1138, which can include information from available sources regarding the navigable area near the TD 101 ( FIG. 20A ). The occupancy grid 1138 can include the probability that an obstacle is present. This information, in conjunction with the SDSF information, can be used to determine whether the SDSF 377 ( FIG. 20C ) can be traversed by the TD 101 ( FIG. 20A ) without encountering an obstacle 1681 ( FIG. 20B ). The system controller 1602 can determine a speed limit 1148 that the TD 101 ( FIG. 20C ) should not exceed, based on environmental and other information. The speed limit 1148 can be used as a guideline for or can override the speed set by the system 1100. The system 1100 can be operably coupled to the base controller 114, which can transmit travel commands 1144 generated by the SDSF controller 1127 to the travel components of the TD 101 ( FIG. 20A ). The proximal controller 114 can provide information to the SDSF controller 1127 about the orientation of the TD 101 (FIG. 20A) during the SDSF traverse.

[0082] 19 , the path line processor 1103 can continuously receive surface classification points 789 in real time, which may include, but are not limited to, points classified as SDSFs. The path line processor 1103 can receive the location of the target point 1139 and the TD location 1141, for example, but not limited to, as indicated by the center 1202 ( FIG. 20A ) of the TD 101 ( FIG. 20A ). The system 1100 can include a polygon processor 1105 that draws a polygon 1147 encompassing the TD location 1141, the location of the target point 1139, and the path 1214 between the target point 1139 and the TD location 1141. The polygon 1147 can include a preselected width. In some configurations, the preselected width can include an approximate width of the TD 101 ( FIG. 20A ). SDSF points 789 that fall within the polygon 1147 can be identified.

[0083] 19 , the SDSF detector 1109 can receive the surface classification points 789, the path 1214, the polygon 1147, and the target points 1139 and can determine the most suitable SDSF line 377 according to the criteria described herein that are available in the incoming data. The SDSF detector 1109 can include, but is not limited to, a point processor 1111 and an SDSF line processor 1113. The point processor 1111 can include selecting two of the SDSF points 789 that are located within the polygon 1147 and drawing an SDSF 377 line between the two points. If there are fewer than a first preselected number of points within a first preselected distance of the SDSF line 377, and if there are fewer than a second preselected number of attempts at selecting an SDSF point 789 to draw a line between the two points and have fewer than the first preselected number of points around the SDSF line, the point processor 1111 may again loop through the select-draw-test loop as described herein. If there are a second preselected number of attempts at selecting an SDSF point to draw a line between them and have fewer than the first preselected number of points around the SDSF line, the point processor 1111 may include concluding that no SDSF line was detected.

[0084] 19 , the SDSF line processor 1113 may include fitting curves 1609-1611 ( FIG. 20A ) to points 789 that fall within a first preselected distance of the SDSF line 377 if there are a first preselected number of points 789 or more. If the number of points 789 that fall within the first preselected distance of the curves 1609-1611 ( FIG. 20A ) exceeds the number of points 789 that fall within the first preselected distance of the SDSF line 377, and if the curves 1609-1611 ( FIG. 20A ) intersect with the path line 1214, and if there are no gaps between points 789 on the curves 1609-1611 ( FIG. 20A ) that exceed a second preselected distance, the SDSF line processor 1113 may include identifying the curves 1609-1611 ( FIG. 20A ) as (for example) the SDSF line 377. If the number of points 789 within a first preselected distance of the curves 1609-1611 (FIG. 20A) does not exceed the number of points 789 within a first preselected distance of the SDSF line 377, or if the curves 1609-1611 (FIG. 20A) do not intersect with the path line 1214, or if there are gaps between points 789 on the curves 1609-1611 (FIG. 20A) that exceed a second preselected distance, and if the SDSF line 377 does not remain stable, and if the curve fit has not been attempted more than a second preselected number of times, the SDSF line processor 1113 may run the curve fit loop again.

[0085] 19 , the SDSF controller 1127 can receive the SDSF line 377, the occupancy grid 1138, the TD orientation change 1142, and the speed limit 1148, and can generate SDSF commands 1144 to navigate the TD 101 ( FIG. 20A ) to properly traverse the SDSF 377 ( FIG. 20C ). The SDSF controller 1127 can include, but is not limited to, an obstacle processor 1115, an SDSF approach 1131, and an SDSF traverse 1133. The obstacle processor 1115 can receive the SDSF line 377, the target point 1139, and the occupancy grid 1138, and can determine, from among the obstacles identified in the occupancy grid 1138, whether any of them may obstruct the TD 101 ( FIG. 20C ) as it traverses the SDSF 377 ( FIG. 20C ). The obstacle processor 1115 may include, but is not limited to, an obstacle selector 1117, an obstacle tester 1119, and a traversal locator 1121. The obstacle selector 1117 may include, but is not limited to, receiving an occupancy grid 1138 as described herein. The obstacle selector 1117 may include selecting occupancy grid points 1608 ( FIG. 20B ) and their associated probabilities from a geographic area common to both the occupancy grid 1138 and the polygon 1147. If the probability that an obstacle exists at the selected grid point 1608 ( FIG. 20B ) is higher than a preselected percentage, and if the obstacle is located between the TD 101 ( FIG. 20A ) and the target point 1139, and if the obstacle is closer than a third preselected distance from the SDSF line 377 between the SDSF line 377 and the target point 1139, the obstacle tester 1119 may include projecting the obstacle onto the SDSF line 377 and forming a projection 1621 that intersects with the SDSF line 377.If the probability that the location contains an obstacle is less than or equal to a preselected percentage, or if an obstacle is not located between TD101 (FIG. 20A) and the target point 1139, or if an obstacle is located at a distance equal to or greater than a third preselected distance from the SDSF line 377 between the SDSF line 377 and the target point 1139, the obstacle tester 1119 may include resuming execution in receiving the occupancy grid 1138 if there are further obstacles to process.

[0086] 19 , the traversal locator 1121 can include connecting the projection points and locating the endpoints 1622 / 1623 ( FIG. 20B ) of the connected projection 1621 ( FIG. 20B ) along the SDSF line 377. The traversal locator 1121 can include marking the portion 1624 ( FIG. 20B ) of the SDSF line 377 between the projection endpoints 1622 / 1623 ( FIG. 20B ) as non-traversable. The traversal locator 1121 can include marking the portion 1626 ( FIG. 20B ) of the SDSF line 377 outside the non-traversable portion 1624 ( FIG. 20B ) as traversable.

[0087] 19 , the SDSF approach 1131 can include transmitting an SDSF command 1144 to turn the TD 101 ( FIG. 20C ) to within a fifth preselected amount perpendicular to the traversable portion 1626 ( FIG. 20C ) of the SDSF line 377. If the heading error relative to a perpendicular line 1627 ( FIG. 20C ), which is perpendicular to the traversable section 1626 ( FIG. 20C ) of the SDSF line 377, exceeds a first preselected amount, the SDSF approach 1131 can include transmitting an SDSF command 1144 to slow the TD 101 ( FIG. 20C ) by a ninth preselected amount. In some configurations, the ninth preselected amount can range from very slow to completely stopped. The SDSF approach 1131 may include transmitting an SDSF command 1144 to drive the TD 101 ( FIG. 20C ) forward toward the SDSF line 377 and transmitting an SDSF command 1144 to slow the TD 101 ( FIG. 20C ) by a second preselected amount per meter traveled. If the distance between the TD 101 ( FIG. 20C ) and the traversable SDSF line 1626 ( FIG. 20C ) is less than a fourth preselected distance, and if the heading error is greater than or equal to a third preselected amount relative to a line perpendicular to the SDSF line 377, the SDSF approach 1131 may include transmitting an SDSF command 1144 to slow the TD 101 ( FIG. 20C ) by a ninth preselected amount.

[0088] 19 , if the heading error is below a third preselected amount relative to a line perpendicular to the SDSF line 377, the SDSF traversal 1133 may include transmitting an SDSF command 1144 to ignore the updated SDSF information and run the TD 101 ( FIG. 20C ) at a preselected rate. If the TD orientation change 1142 indicates that the climb of the leading edge 1701 ( FIG. 20C ) of the TD 101 ( FIG. 20C ) relative to the trailing edge 1703 ( FIG. 20C ) of the TD 101 ( FIG. 20C ) is between a sixth preselected amount and a fifth preselected amount, the SDSF traversal 1133 may include transmitting an SDSF command 1144 to run the TD 101 ( FIG. 20C ) forward and to increase the speed of the TD 101 ( FIG. 20C ) to a preselected rate according to the degree of climb. If the TD orientation change 1142 indicates that the elevation of the leading edge 1701 ( FIG. 20C ) relative to the trailing edge 1703 ( FIG. 20C ) of the TD 101 ( FIG. 20C ) is less than a sixth preselected amount, the SDSF traversal 1133 may include sending an SDSF command 1144 to run the TD 101 ( FIG. 20C ) forward at a seventh preselected speed. If the TD location 1141 indicates that the trailing edge 1703 ( FIG. 20C ) is more than a fifth preselected distance from the SDSF line 377, the SDSF traversal 1133 may include acknowledging that the TD 101 ( FIG. 20C ) has completed traversing the SDSF 377. If the TD location 1141 indicates that the trailing edge 1703 (FIG. 20C) is less than or equal to the fifth preselected distance from the SDSF line 377, the SDSF traversal 1133 may include executing the loop again beginning with ignoring the updated SDSF information.

[0089] Some example ranges for the preselected values ​​described herein may include, but are not limited to, those outlined in Table I. [Table 1]

[0090] 21 , to support real-time data collection, in some configurations, a system of the present teachings can generate locations in three-dimensional space of various surface types in response to receiving data such as, for example, but not limited to, RGD-D camera image data. The system can rotate images 2155 and transform them from a camera coordinate system 2157 to a UTM coordinate system 2159. The system can generate a polygon file from the transformed images, which can represent three-dimensional locations associated with the surface type 2161. A method 2150 of locating features 2151 from camera images 2155 received by a TD 101 having a pose 2163 can include, but is not limited to, receiving, by the TD 101, camera images 2155. Each of the camera images 2155 can include an image timestamp 2171, and each of the images 2155 can include image color pixels 2167 and image depth pixels 2169. The method 2150 can include receiving a pose 2163 of the TD 101, the pose 2163 having a pose timestamp 2171, and determining a selected image 2173 by identifying an image from the camera images 2155 having an image timestamp 2165 that is closest to the pose timestamp 2171. The method 2150 can include separating image color pixels 2167 from image depth pixels 2169 in the selected image 2173, and determining an image surface classification 2161 for the selected image 2173 by providing the image color pixels 2167 to a first machine learning model 2177 and providing the image depth pixels 2169 to a second machine learning model 2179. The method 2150 may include determining perimeter points 2181 of a feature in the camera image 2173, the feature including feature pixels 2151 within the perimeter, each of the feature pixels 2151 having the same surface classification 2161, and each of the perimeter points 2181 may have a coordinate set 2157. The method 2150 may include converting each of the coordinate sets 2157 to UTM coordinates 2159.

[0091] The present teachings are directed to computer systems for performing the methods discussed in the description herein, and to computer-readable media containing programs for performing these methods. Raw data and results can be stored, printed, displayed, transferred to another computer, and / or transferred to another location for future retrieval and processing. Communication links can be wired or wireless, using, for example, cellular, military, and satellite communication systems. Portions of the system can run on computers with varying numbers of CPUs. Other alternative computer platforms can also be used.

[0092] The present configuration also covers software / firmware / hardware for performing the methods discussed herein, and computer-readable media storing software for performing these methods. The various modules described herein can be performed on the same CPU or on different CPUs. In accordance with statute, the present configuration has been described in more or less specific language with respect to structural and method features. However, it should be understood that the present configuration is not limited to the specific features shown and described, since the means disclosed herein comprise preferred forms for embodying the present configuration.

[0093] The method can be implemented, in whole or in part, electronically. Signals representing actions taken by elements of the system and other disclosed configurations can travel via at least one live communications network. Control and data information can be executed and stored electronically on at least one computer-readable medium. The system can be implemented to run on at least one computer node in at least one live communications network. Typical forms of the at least one computer-readable medium can include, for example, but are not limited to, a floppy disk, a flexible disk, a hard disk, a magnetic tape, or any other magnetic medium, a compact disk read-only memory or any other optical medium, a punch card, a paper tape, or any other physical medium with a pattern of holes, a random access memory, a programmable read-only memory, and an erasable programmable read-only memory (EPROM), a flash EPROM, or any other memory chip or cartridge, or any other medium from which a computer can read. Additionally, at least one computer-readable medium may contain graphs in any format, including, but not limited to, Graphics Interchange Format (GIF), Joint Photographic Experts Group (JPEG), Portable Network Graphics (PNG), Scalable Vector Graphics (SVG), and Tagged Image File Format (TIFF), as appropriate, subject to an appropriate license.

[0094] While the present teachings have been described above in terms of specific configurations, it should be understood that they are not limited to these disclosed configurations. Numerous modifications and other configurations will occur to those skilled in the art to which this pertains and are intended to be and are covered by both this disclosure and the appended claims. It is intended that the scope of the present teachings should be determined by the proper interpretation and construction of the appended claims and their legal equivalents, as understood by those skilled in the art relying on the disclosure in this specification and the accompanying drawings.

Claims

1. 1. A method of navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The method comprises: accessing a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; transforming the point cloud data into a global coordinate system; determining a boundary of the at least one substantially discontinuous surface feature (SDSF); creating an SDSF buffer of a preselected size around said boundary; determining which of the at least one SDSF is traversable based at least on at least one SDSF traversal criterion; Including, the transformed point cloud data includes an edge graph indicating edges of the at least one graphed polygon, the edge graph including initial weights; While the TD is traveling along the route, the initial weights are adjusted, and the route is modified using the traversable SDSF and the adjusted weights. method.

2. The at least one SDSF crossing criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; The method of claim 1 , wherein a substantially 90° approach for crossing the at least one SDSF that satisfies the at least one SDSF crossing criterion is calculated.

3. 1. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point, the system comprising: a first processor that accesses a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; a second processor for transforming the point cloud data into a global coordinate system; a third processor that determines a boundary of the at least one SDSF, the third processor creating an SDSF buffer of a preselected size around the boundary; a fourth processor that determines which of the at least one SDSF is traversable based on at least one SDSF traversal criterion; the transformed point cloud data includes an edge graph indicating edges of the at least one graphed polygon, the edge graph including initial weights; While the TD is traveling along the route, the initial weights are adjusted, and the route is modified using the traversable SDSF and the adjusted weights. system.

4. The at least one SDSF crossing criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; The system of claim 3 , wherein an approximately 90° approach for crossing the at least one SDSF that satisfies the at least one SDSF crossing criterion is calculated.

5. 1. A method of navigating at least one substantially discontinuous surface feature (SDSF) encountered by a TD, wherein the TD travels a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The method comprises: accessing point cloud data representing the surface; filtering the point cloud data; forming the filtered point cloud data into a processable portion; merging the processable portions into at least one concave polygon; Locating and labeling the at least one SDSF within the at least one concave polygon, wherein the locating and labeling forms labeled point cloud data; creating a graphing polygon based at least on the at least one concave polygon, the graphing polygon forming a root form; transforming the point cloud data into a global coordinate system; determining a boundary of said at least one SDSF; creating an SDSF buffer of a preselected size around said boundary; determining which of the at least one SDSF is traversable based on at least one SDSF traversal criterion; the transformed point cloud data includes an edge graph indicating edges of the at least one graphed polygon, the edge graph including initial weights; While the TD is traveling along the route, the initial weights are adjusted, and the route is modified using the traversable SDSF and the adjusted weights. method.

6. filtering the point cloud data removing points representing transient objects and outliers from the point cloud data; replacing the removed points with a preselected height; and The method of claim 5 , comprising:

7. forming the filtered point cloud data into a processable portion, Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; The method of claim 5 , comprising:

8. Merging the processable portions comprises: reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; The method of claim 5 , comprising:

9. Locating and labeling the at least one SDSF comprises: sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; locating at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion of a plurality of criteria, the plurality of criteria including: i) a minimum number of points in a first category of the at least three categories of points and a minimum number of points in a second category of the at least three categories of points; ii) a majority of points in the point cloud data being located within opposing hemispheres of the point cloud data; and iii) the point cloud data including at least two categories of points in the at least three categories of points; The method of claim 8, comprising:

10. 10. The method of claim 9, further comprising creating at least one SDSF trajectory based at least on whether a plurality of the at least one SDSF points, in combination, satisfy at least one second preselected criterion of the plurality of criteria.

11. Creating the graphed polygon comprises: creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion of the plurality of criteria; The method of claim 10 further comprising:

12. smoothing the outer edge includes trimming the outer edge outward to form an outer edge; The method of claim 11.

13. The method of claim 12 , wherein forming the running margin of the smoothed outer edge comprises trimming the outer edge inward.

14. The at least one SDSF crossing criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; The method of claim 5 , wherein an approximately 90° approach for crossing the at least one SDSF that meets the at least one SDSF crossing criterion is calculated.

15. 1. A system for navigating at least one substantially discontinuous surface feature (SDSF) encountered by a transport device (TD), wherein the TD navigates a path on a surface, the surface including the at least one SDSF, the path including a start point and an end point; The system comprises: a first processor that accesses point cloud data representing the surface; a first filter for filtering the point cloud data; a second processor for forming a processable portion from the filtered point cloud data; a third processor for merging the processable portions into at least one concave polygon; a fourth processor for locating and labeling the at least one SDSF within the at least one concave polygon, the locating and labeling forming labeled point cloud data; a fifth processor for creating graphed polygons; a sixth processor that accesses a route configuration, the route configuration including at least one graphed polygon including filtered point cloud data, the filtered point cloud data including labeled features, and the point cloud data including a drivable margin; a seventh processor that transforms the point cloud data into a global coordinate system; an eighth processor that determines a boundary of the at least one SDSF, the eighth processor creating an SDSF buffer of a preselected size around the boundary; a ninth processor that determines which of the at least one SDSF is traversable based on at least one SDSF traversal criterion; the transformed point cloud data includes an edge graph indicating edges of the at least one graphed polygon, the edge graph including initial weights; While the TD is traveling along the route, the initial weights are adjusted, and the route is modified using the traversable SDSF and the adjusted weights. system.

16. The first filter comprises: removing points representing transient objects and outliers from the point cloud data; replacing the removed points with a preselected height; and 16. The system of claim 15, comprising executable code comprising:

17. The second processor Dividing the point cloud data into the processable portions; removing points of a preselected height from said addressable portion; 16. The system of claim 15, comprising executable code comprising:

18. The third processor reducing the size of the processable portion by analyzing outliers, voxels, and normals; Enlarging an area from the reduced-size addressable portion; and determining an initial drivable surface from the expanded area; and Segmenting and meshing the initial drivable surface; Identifying polygon locations within the segmented and meshed initial drivable surface; establishing at least one drivable surface based at least on said polygon; 16. The system of claim 15, comprising executable code comprising:

19. The fourth processor sorting the point cloud data of the drivable surface according to an SDSF filter, the SDSF filter including at least three categories of points; locating at least one SDSF point based on whether the at least three categories of points, in combination, satisfy at least one first preselected criterion of a plurality of criteria, the plurality of criteria including: i) a minimum number of points in a first category of the at least three categories of points and a minimum number of points in a second category of the at least three categories of points; ii) a majority of points in the point cloud data being located within opposing hemispheres of the point cloud data; and iii) the point cloud data including at least two categories of points in the at least three categories of points; 20. The system of claim 18, comprising executable code comprising:

20. 20. The system of claim 19, comprising executable code that includes at least generating at least one SDSF trajectory based on whether a plurality of the at least one SDSF point, in combination, meets at least one second preselected criterion of the plurality of criteria.

21. The step of creating the graphed polygon comprises: creating at least one convex polygon from the at least one drivable surface, the at least one convex polygon including an outer edge; smoothing the outer edge; forming a running margin based on the smoothed outer edge; adding the at least one SDSF track to the at least one drivable surface; removing an inner edge from the at least one drivable surface according to at least one third preselected criterion of the plurality of criteria; 21. The system of claim 20, comprising executable code comprising:

22. 22. The system of claim 21, wherein smoothing the outer edge comprises executable code that includes trimming the outer edge outward to form an outer edge.

23. 23. The system of claim 22, wherein forming the running margin of the smoothed outer edge comprises executable code that includes trimming the outer edge inward.

24. The at least one SDSF crossing criterion is: a preselected width of the at least one SDSF and a preselected smoothness of the at least one SDSF; The system of claim 15 , wherein an approximately 90° approach for crossing the at least one SDSF that satisfies the at least one SDSF crossing criterion is calculated.

Images