Vehicle Navigation

Abstract

To provide a method used for navigation of a vehicle in an environment.SOLUTION: In one or more electronic processing devices, a method includes steps of: acquiring 100, from a distance sensor mounted on a vehicle, distance data indicating a distance between the distance sensor and a detection part of an environment while the vehicle crosses the environment; generating 110 mapping data indicating a three-dimensional map of the environment and location data indicating one or more distance sensor locations in the environment by analyzing the distance data; identifying 120 a field of view cutoff part of the environment using the mapping data and the location data; generating 130 a virtual surface based on the field of view cutoff part of the environment; and calculating 140 a path for which navigation in the environment is possible using at least part of the virtual surface.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to systems and methods for use in navigating a vehicle within an environment, and in one particular example, to systems and methods for navigation planning in unstructured environments that include unobservable features such as recessed obstacles and / or blocked areas. [Background technology]

[0002] Any reference in this specification to any prior publication (or information derived from a prior publication) or to any matter known to the public is not, and must not be taken as, an acknowledgement or admission or suggestion in any way that the prior publication (or information derived from a prior publication) or matter known to the public forms part of the common general knowledge in the field of activity to which this specification pertains.

[0003] Concave obstacles such as cliffs, grooves, depressions and / or blocked areas in the environment pose difficult problems for autonomous navigation, as reported for example in (Non-Patent Document 1) or (Non-Patent Document 2).

[0004] Concave obstacles are difficult for vehicle-mounted sensors to detect due to obstruction of the view by nearby terrain, such as steep slopes, inclines, and / or protrusions on the drop-off. Compared to convex obstacles, the obstruction and viewing angle result in fewer target pixels, which in turn reduces the effective detection range, often to within the stopping distance of a ground vehicle moving at a significant speed.

[0005] Extreme slope angles (i.e., 45 oFor vehicles capable of autonomously traversing terrain (larger than 100 meters), even slowly approaching a concave obstacle requires considerable care in planning and sensor placement to ensure robust navigation. That is, the obstacle must be approached from an optimal angle so the system can look down and determine whether the terrain is a traversable slope or a critical discontinuity. There are few examples of robotic systems that can detect and traverse concave obstacles in unstructured terrain, where the system can both detect and safely traverse gaps in the terrain by reasoning about the boundaries of the gap or unsurveyed area.

[0006] To effectively handle concave and convex obstacles, it is important to have an efficient and accurate height map representation that encapsulates the height of each grid cell relative to the odometry coordinates used to estimate slope and terrain characteristics. The height map is used to derive a cost map (including convex and concave obstacles), which is then used to compute a trajectory to safely reach the goal, taking into account not only the geometric details of the path but also the vehicle's kinematic constraints to generate a feasible trajectory.

[0007] (Non-Patent Document 3) describes a popular library that utilizes probabilistic and 3D octree representations for memory efficiency. Octomap has been successfully deployed in many different applications, including underwater, flying, walking, and terrestrial robots.

[0008] Convex obstacles, such as boxes or other robots, can be effectively detected by computing eigenvectors of small patches from the estimated height map and the current vehicle state (such as pose and surrounding meta-information), as described in [4]. In contrast, concave obstacles are often unobservable (e.g., cliffs or ditches) and are inferred from gaps in the map, so detection using geometric information is widespread. Depth data is accumulated in voxel maps, and unobserved cells are often computed and then classified as obstacles based on neighboring observed voxels.

[0009] (Non-Patent Document 5) and (Non-Patent Document 2) use ray tracing on a 2D map to calculate the minimum possible slope and maximum (downward) step height of an unobserved area. (Non-Patent Document 6) and (Non-Patent Document 7) use a LiDAR sensor with 3D ray tracing to determine visibility occlusions and classify obstacles using nearby observed voxels, or classify points down the ray path using heuristics and a support vector machine (SVM).

[0010] (Non-Patent Document 8) describes using LiDAR-based detection in 2D height maps by propagating information from all nearby observed cells to infer unobserved terrain. Image-based methods are by far the rarer method for detecting concave obstacles, but thermal imagery has been exploited by observing that at night, depressed depressions remain warmer than the surrounding terrain, as described in (Non-Patent Document 9). [Prior art documents] [Non-patent literature]

[0011] [Non-Patent Document 1] A. Stentz, J. Bares, T. Pilarski, and D. Stager, “The crusher system for autonomous navigation,” AUVSIs Unmanned Systems North America, vol. 3, 2007 [Non-patent document 2] M. Bajracharya, J. Ma, M. Malchano, A. Perkins, AARizzi, and L. Matthies,“High Fidelity Day / Night Stereo Mapping with Vegetation and Negative Obstacle Detection for Vision-in-the-Loop Walking” Systems,2013,pp.3663-3670

Outdoor Tools3

Outdoor Tools 4

Direct Environment 5

Outdoor Configuration6

Direct Environment 7

Non-Patent Document 8

Non-Patent Document 9

Non-Patent Document 10

Non-Patent Document 11

Outdoor Tools 12

Outdoor Content13

Outdoor Tools 14

Outdoor Tools 15

Outdoor Content 16

[0012] In one broad form, an aspect of the invention seeks to provide a method for use in navigating a vehicle within an environment, the method including: acquiring, in one or more electronic processing devices, distance data from distance sensors mounted on the vehicle while the vehicle traverses the environment, the distance data indicating distances between the distance sensors and sensed portions of the environment; analyzing the distance data to generate mapping data indicating a three-dimensional map of the environment and position data indicating positions of one or more distance sensors within the environment; identifying view-obstructed portions of the environment using the mapping data and the position data; generating a virtual surface based on the view-obstructed portions of the environment; and calculating a navigable path within the environment using, at least in part, the virtual surface.

[0013] In one broad form, an aspect of the invention seeks to provide a system for use in navigating a vehicle within an environment, the system including one or more electronic processing devices configured to: acquire distance data from a distance sensor mounted on the vehicle while the vehicle traverses the environment, the distance data indicating a distance between the distance sensor and a sensed portion of the environment; analyze the distance data to generate mapping data indicating a three-dimensional map of the environment and position data indicating one or more distance sensor positions within the environment; identify view-obstructed portions of the environment using the mapping data and the position data; generate a virtual surface based on the view-obstructed portions of the environment; and calculate a navigable path within the environment using, at least in part, the virtual surface.

[0014] In one broad form, an aspect of the present invention seeks to provide a computer program product for use in navigating a vehicle within an environment, the computer program product including computer-executable code that, when executed by one or more suitable programmed electronic processing devices, causes the one or more processing devices to: obtain distance data from distance sensors mounted on the vehicle while the vehicle traverses the environment, the distance data indicating distances between the distance sensors and sensed portions of the environment; analyze the distance data to generate mapping data indicating a three-dimensional map of the environment and position data indicating positions of the one or more distance sensors within the environment; identify view-obstructed portions of the environment using the mapping data and the position data; generate a virtual surface based on the view-obstructed portions of the environment; and calculate a navigable path within the environment using, at least in part, the virtual surface.

[0015] In one embodiment, the method further includes controlling, at the one or more electronic processing devices, the vehicle to move the vehicle through the environment at least in part using the navigable path.

[0016] In one embodiment, the method further includes, at the one or more electronic processing devices, reducing the vehicle speed while the vehicle is approaching the virtual surface.

[0017] In one embodiment, the method further includes, at the one or more electronic processing devices, updating the virtual surface using the acquired distance data while the vehicle is moving within the environment, and recalculating a navigable path at least in part using the updated virtual surface.

[0018] In one embodiment, the method further includes, in the one or more electronic processing devices, using the mapping data to generate a three-dimensional occupancy grid representing locations of detected portions of the environment within the three-dimensional volume, and using the occupancy grid to identify view-blocking portions of the environment.

[0019] In one embodiment, the method further includes, at the one or more electronic processing devices, updating an occupancy grid using the acquired distance data while the vehicle is moving within the environment, and recalculating a navigable path at least in part using the updated occupancy grid.

[0020] In one embodiment, the method further includes, in the one or more electronic processing devices, identifying view-blocking portions of the environment using projections between the range sensor positions and corresponding sensed portions of the environment.

[0021] In one embodiment, the method further includes, at the one or more electronic processing devices, populating an occupancy grid with occupied voxels using sensed portions of the environment, populating the occupancy grid with free voxels based on the projection, and, for each vertical column of the occupancy grid, populating the occupancy grid with virtual surface voxels, wherein, for each vertical column of the occupancy grid, in populating the occupancy grid with virtual surface voxels, the free voxels are positioned above the unobserved voxels and no observed voxels exist below the free voxels.

[0022] In one embodiment, the method further includes, in the one or more electronic processing devices, identifying a virtual surface based on adjacent virtual surface voxels and calculating a navigable path using the virtual surface.

[0023] In one embodiment, the method further includes, in the one or more electronic processing devices, calculating a slope of the virtual surface and determining whether the virtual surface is traversable based on the slope.

[0024] In one embodiment, the method further includes, at the one or more electronic processing devices, recalculating the virtual surface based on updates to the occupancy grid while the vehicle is moving within the environment.

[0025] In one embodiment, the method further includes, at the one or more electronic processing devices, determining a vehicle clearance and calculating a navigable route at least in part using the vehicle clearance.

[0026] In one embodiment, the method further includes, in the one or more electronic processing devices, comparing the vehicle clearance with the height of consecutive free voxels for a column of the occupancy grid, and determining whether the column is traversable based on a result of the comparison.

[0027] In one embodiment, the method further includes generating, at the one or more electronic processing devices, a height map indicative of surface heights in the environment using the populated occupancy grid, and calculating a navigable path using the height map.

[0028] In one embodiment, the method further includes generating, at the one or more electronic processing devices, a cost map indicating costs associated with traversals of the environment, and calculating a navigable path using the cost map.

[0029] In one embodiment, the method further includes generating, at the one or more electronic processing devices, a cost map using the height map by labeling non-traversable obstacles in the height map.

[0030] It will be understood that the broad aspects of the invention and their respective features can be used in combination and / or independently. Reference to separate broad aspects is not intended to be limiting. Moreover, it will be understood that a system or device can be used to perform method features, or a method can be used to implement system or device features.

[0031] Various examples and embodiments of the present invention will now be described with reference to the accompanying drawings. [Brief explanation of the drawings]

[0032] [Figure 1] 1 is a flowchart of an example method for navigating a vehicle within an environment. [Figure 2] 1 is a schematic diagram of an example vehicle. [Figure 3A] 1 is a schematic side view of an example vehicle approaching a concave obstacle. FIG. [Figure 3B] 1 is a schematic side view of an example vehicle approaching an area blocked by a convex obstacle; FIG. [Figure 3C] FIG. 3C is a schematic plan view of the example of FIG. 3B. [Figure 4A-B] 10 is a flowchart of a further example of a method for navigating a vehicle within an environment. [Figure 5] FIG. 10 is a schematic side view of an example of a column of an occupancy grid used for height map generation. [Figure 6] 1 is a schematic side view of an example vehicle near the edge of a concave obstacle. [Figure 7A-C] 1A-1C are schematic side views of example height maps generated by a vehicle gradually approaching the edges of concave obstacles of different slopes. [Figure 8A] 1 is a schematic diagram of an example scenario in which an area is blocked by a convex obstacle; [Figure 8B] FIG. 8B is a schematic diagram of an example cost map for the scenario of FIG. 8A. [Figure 9A] FIG. 1 is a schematic diagram of an example scenario involving a concave obstacle. [Figure 9B] FIG. 9B is a schematic diagram of an example cost map for the scenario of FIG. 9A. [Figure 10] FIG. 1 is a schematic diagram of an example system architecture. [Figure 11A] 1 is an example image of a vehicle approaching a first terrain feature that includes a concave obstacle. [Figure 11B-D] 11B is a schematic diagram of a series of cost maps generated while the vehicle traverses the first terrain of FIG. 11A. [Figure 11E] 10 is an example image of a vehicle approaching a second terrain that includes a concave obstacle. [Figure 11F-H] FIG. 11F is a schematic diagram of a series of cost maps generated while the vehicle traverses the second terrain of FIG. 11E. [Figure 11I] 10 is an example image of a vehicle approaching a third terrain feature that includes a concave obstacle. [Figure 11J-L] FIG. 11I is a schematic diagram of a series of cost maps generated while the vehicle traverses the third terrain of FIG. [Figure 12A] 1 is a schematic diagram of an example of a vehicle approaching a deadly ditch. [Figure 12B-G] FIG. 12B is a schematic diagram of an example cost map for approach for crossing the deadly chasm of FIG. 12A. [Figure 13A] 1 is a schematic diagram of an example of a vehicle approaching a deadly ditch with a navigable slope. [Figure 13B-G] FIG. 13B is a schematic diagram of an example cost map for approaches for crossing the deadly ditch and navigable slope of FIG. 13A. DETAILED DESCRIPTION OF THE INVENTION

[0033] Referring now to FIG. 1, an example process for use in navigating a vehicle within an environment will be described.

[0034] The environment is typically unstructured and may be a natural environment, including an open environment (such as an outdoor area) or an enclosed environment (such as a cave system or the like). Additionally and / or alternatively, the environment may be a structured environment (such as a building, underground mine or the like) or a combination of natural and structured environments.

[0035] For purposes of this example, a vehicle is assumed to be any device capable of traversing an environment and may include an autonomous vehicle, a robot, or the like. Vehicles may use a variety of different locomotion mechanisms depending on the environment and may include wheels, tracks, or legs. It will be understood, therefore, that the term vehicle should be taken broadly and not be construed as limited to a particular type of vehicle.

[0036] Regardless of the nature of the vehicle, vehicles typically include distance sensors, such as LiDAR sensors, stereoscopic imaging systems, or the like. Additionally, each vehicle typically includes one or more electronic processing devices configured to receive signals from the distance sensors and process or provide these signals to a remote processing device for processing and analysis. In one particular example, this operation involves implementing a SLAM (Simultaneous Localization And Mapping) type algorithm to simultaneously perform localization and mapping. The processing device may be of any suitable form, including a microprocessor, a microchip processor, a logic gate configuration, firmware optionally associated with an implementation of logic such as an FPGA (Field Programmable Gate Array), or any other electronic device, system, or arrangement. This process may be performed using multiple processing devices, with processing distributed among one or more devices as needed; thus, for example, some processing may be performed on-board the vehicle and other processing may be performed remotely. Nevertheless, for ease of illustration, the examples below refer to a single processing device, but it will be appreciated that reference to a singular processing device should be understood to encompass multiple processing devices (with processing appropriately distributed between devices) and vice versa.

[0037] In this example, the navigation process involves acquiring distance data from a distance sensor mounted on the vehicle while the vehicle traverses an environment, at step 100. The distance data indicates the distance between the distance sensor and a sensed portion of the environment.

[0038] In step 110, the processing device analyzes the range data to generate mapping data indicative of a three-dimensional map of the environment and position data indicative of one or more range sensor positions within the environment. The mapping data typically represents a three-dimensional representation of the environment and may be in the form of a point cloud or similar, and the position data indicates the positions of the range sensors within the environment when each range datum is captured. The mapping data and position data are generated by processing the range data collected by the range sensors, for example using a SLAM algorithm or the like.

[0039] In step 120, the processing device uses the mapping data and position data to identify occluded portions of the environment. In this regard, portions of the three-dimensional volume in which the environment resides may be considered occupied. Ray tracing from the range sensor locations to the sensed portions of the environment may then be used to identify intervening unoccupied or "free" portions of the environment; any portions of the environment that are unoccupied or free may be considered occluded. In one example, this process is performed using an occupancy grid, an example of which is described in more detail below.

[0040] In step 130, the processing device generates a virtual surface based on the view occlusions of the environment, for example, by combining adjacent view occlusion points to create the virtual surface. The resulting virtual surface can then be considered a real surface in the context of path planning, such that a navigable path through the environment can be calculated using, at least in part, the view occlusions of the environment in step 140. In one particular example, the planning process is performed by generating a height map and a cost map, and a navigable path is calculated using the resulting cost map, as described in more detail below.

[0041] Once the path is created, the path can be used to control the vehicle so that it can navigate through the environment, in step 150. It will be appreciated that as the vehicle continues to move through the environment, additional distance data is acquired and the above process is repeated using this distance data. In particular, observations of previously obstructed portions of the environment can cause the processing device to update the virtual surface while the vehicle is moving through the environment, thereby refining the path planning to take into account the updated surface.

[0042] Thus, the process described above operates to generate a map of the environment using distance data obtained from the distance sensor, and the generated map of the environment is then analyzed to identify view-obstructed portions of the environment. These view-obstructed portions of the environment are then used to derive virtual surfaces, which can be used as potentially traversable surfaces in the navigation planning process. As the vehicle moves through the environment, additional data is collected, which can refine the virtual surfaces (e.g., depending on the situation, these virtual surfaces can be converted to actual surfaces or evaluated as untraversable when a corresponding environment is detected).

[0043] This approach provides greater flexibility in route planning, in particular by avoiding the evaluation of view-blocked portions of the environment as untraversable, thereby limiting the ability to successfully execute a route plan. This approach allows a wider range of traversable routes to be calculated, thereby reducing the computation, time, and operations required to successfully traverse the environment. Additionally, this approach significantly increases the likelihood of a traversable route being correctly identified, increasing the vehicle's ability to traverse unstructured environments and reducing the risk of the vehicle becoming bogged down within the environment.

[0044] Many additional features are described herein.

[0045] An example vehicle is shown in more detail in FIG.

[0046] In this example, vehicle 200 includes a chassis and body 210 having at least one electronic processing device 211 located thereon coupled to a mapping system 212 configured to perform a scan of the environment around the vehicle to construct a 3D map (i.e., a point cloud) of the environment. In one example, the mapping system includes a 3D LiDAR sensor, such as a VLP-16 3D LiDAR produced by Velodyne. Processing device 211 may also be coupled to an inertial sensing device 213, such as an IMU (Inertial Measurement Unit), a control system 214 to enable control of the movement of the vehicle, and one or more other sensors 215. The sensors 215 may include proximity sensors for additional safety control, or imaging devices or the like to enable capture of images of the environment, for example, for purposes of coloring a point cloud representation of the environment.

[0047] The processing device 211 may also be connected to an external interface 216 (such as a wireless interface) to enable wireless communication with other vehicles, for example via one or more communication networks (such as a mobile communication network, a 4G or 5G network, a WiFi network, etc.) or via a direct point-to-point connection (such as Bluetooth) or the like.

[0048] The electronic processing device 211 is also coupled to memory 217, which stores application software executable by the processing device 211 to perform the necessary processes. The application software may include one or more software modules and may be executed in a suitable execution environment, such as an operating system environment or the like. The memory 217 may also be configured to store mapping data and frame data as needed, as well as any generated maps. It will be appreciated that the memory may include volatile memory, non-volatile memory, or a combination thereof, as needed.

[0049] It will be appreciated that the configuration described above, assumed for purposes of the following example, is not essential and many other configurations can be used. For example, in this example, the vehicle is shown as a wheeled vehicle, but it will be appreciated that this is not essential and a wide variety of vehicles and mobility systems can be used.

[0050] Here, examples of the visibility limiting feature will be described with reference to FIGS. 3A to 3C.

[0051] 3A shows a downward slope 301. In this example, as vehicle 200 approaches the slope, the location of position sensor 212 on vehicle 200 prevents direct imaging of the slope, and instead results in an obstructed view area 302. It will be appreciated that in this example, as the vehicle approaches slope 301, obstructed view area 302 decreases in size and, if the slope is not very steep, eventually disappears by the time the slope surface is imaged. Thus, treating obstructed view area 302 as a virtual surface allows for planning a path approaching the slope, with the path being refined once the slope is detected and / or the virtual surface becomes too steep to be traversed.

[0052] 3B and 3C, convex obstacle 311 restricts the visibility of the volume behind the obstacle and also results in a view-blocking region 312. Again, by creating a virtual surface behind convex obstacle 311, a vehicle can approach this region by going around convex obstacle 311 and then detect the view-blocking volume.

[0053] It will be appreciated that in either case, the processing device may control the movement of the vehicle while it is approaching the virtual surface, for example by slowing the vehicle speed, so that the virtual surface is refined and / or may adapt the vehicle's control once the actual surface is detected, which may be used to redirect the vehicle if the surface is found to be unnavigable.

[0054] In one example, the techniques described above use a three-dimensional occupancy grid generated by a processing device using the mapping data. The occupancy grid represents the location of the sensed portion of the environment within the three-dimensional volume and typically comprises a three-dimensional grid of voxels, with each voxel labeled based on whether the voxel matches the sensed portion of the environment, for example, by labeling voxels that match the environment as occupied.

[0055] The processing device typically uses projections between range sensor locations and corresponding sensed portions of the environment to identify unobstructed, particularly free, portions of the environment. Rays traced between occupied voxels and range sensor locations when corresponding portions of the environment are sensed can thus label intervening voxels as unoccupied or free voxels. Thus, initially, the sensed portions of the environment can be used to populate the occupancy grid with occupied voxels, and based on the projections, the occupancy grid can be populated with free voxels. Any remaining voxels are then considered unobstructed or obscured and can then be used in generating the virtual surface.

[0056] Once the occupancy grid is populated, the grid is further analyzed to identify virtual surface voxels. This analysis is typically accomplished by identifying boundaries between free voxels and unobserved voxels. For example, each column of the occupancy grid can be examined to convert unobserved voxels into virtual surface voxels, with free voxels located above the unobserved voxels and no observed voxels below the free voxels.

[0057] Following this, the processing device can identify a virtual surface using neighboring virtual surface voxels, for example by creating a surface that spans the nearest virtual surface voxels, and then calculate a navigable path using the virtual surface. In one example, this operation involves treating the virtual surface as a real surface in a path planning algorithm, which means that the processing device calculates the slope of the virtual surface and determines whether the virtual surface is potentially traversable based on the slope.

[0058] The process can be performed iteratively while the vehicle moves through the environment, and the occupancy grid is therefore updated as the vehicle moves through the environment. Thus, as new portions of the environment are detected, the occupied voxels and free voxels are updated, thereby allowing the processing device to recalculate the virtual surface based on the updated occupancy grid. Subsequently, the path planning algorithm can refine the calculated navigable path. Thus, the processing device can use the acquired distance data to update the occupancy grid and recalculate the navigable path at least in part using the updated occupancy grid. For example, as the vehicle approaches a concave obstacle (such as a downward slope), visibility of the view-blocking area increases, thereby enabling a refinement of the potential steepness of the slope, which in turn can improve the assessment of whether the slope is navigable.

[0059] In addition to inspecting the virtual surface, the processing device may also be configured to determine vehicle clearance and calculate a navigable path using, at least in part, the vehicle clearance, thereby preventing attempts by the vehicle to traverse gaps that are too low for the vehicle. Again, this step may be performed using an occupancy grid by, for each column of the occupancy grid, comparing the vehicle clearance with the height of successive free voxels and then determining whether the column is traversable based on the results of the comparison. This advantageously allows the same occupancy grid to be used to account for unobservable features and vehicle clearance.

[0060] In one example, the processing device uses the populated occupancy grid to generate a height map, which indicates surface heights in the environment, including virtual surface heights. The height map can then be used in calculating a navigable path. In one particular example, this latter step is accomplished by using the height map to generate a cost map indicating costs associated with traversing portions of the environment, and then using the cost map to calculate a navigable path. Thus, whereas the height map simply determines the heights of various surfaces, the cost map is generated by analyzing surfaces and determining whether they are navigable. For example, a surface that is too steep for a vehicle to traverse is labeled as untraversable, preventing its use in path planning.

[0061] A further example of a route planning process will now be described in more detail with reference to Figures 4A and 4B.

[0062] In this example, range data is obtained in step 400 and mapping and position data is generated in step 405 in a manner substantially similar to that described above with respect to steps 100 and 110 .

[0063] In step 410, a blank occupancy grid is generated, with each voxel labeled as unobserved. Then, in step 415, the processing device populates the grid with occupied voxels based on the locations of the sensed portions of the environment, effectively overwriting the unobserved labels with occupied labels, where the voxels are aligned with the sensed portions of the environment. Next, in step 420, the processing device performs ray tracing, extending rays between the location of the range sensor and the portion of the environment sensed when the range sensor is located at that location. This ray tracing is used to identify portions of the environment that are observed (but unoccupied), and the corresponding voxels are updated and labeled as free voxels. Upon completion of this step, the occupancy grid should be fully populated with occupied, free, and unobserved voxels.

[0064] In step 425, the processing device examines the columns of voxels in the occupancy grid, and for each column, in step 430, it determines whether there is an obstructed portion of the environment based on the voxels labeled as unobserved. Then, in step 435, it uses these voxels to identify a virtual surface, which is accomplished by labeling the free voxel directly above the unobserved voxel as a virtual surface voxel. Having completed this process for all columns containing unobserved voxels, the processing device generates a height map in step 440 using the resulting occupancy grid. The height map is effectively a map of the heights of any surfaces (including observed and virtual surfaces) in the occupancy grid.

[0065] In step 445, the processing device analyzes the height map, e.g., examining surface slopes and comparing these surface slopes with information regarding the vehicle's ability to traverse different terrains, to identify any surfaces that are not traversable. Then, in step 450, the height map is populated with information regarding whether a surface is traversable to form a cost map. These non-traversable surfaces are thus given a cost of infinity, effectively eliminating these surfaces from use as part of a navigable path. It will also be appreciated that in addition to a simple binary assessment of whether a surface is traversable, the cost may include additional levels of refinement, such as labeling surfaces based on how steep and potentially dangerous traversal of these surfaces is.

[0066] Once generated, the processing device uses the cost map in calculating one or more navigable paths using a path-planning algorithm, step 455. Such algorithms are known in the art and are described in more detail below. Following calculation of one or more paths, these paths can be used to control the vehicle, step 460, so that the vehicle can move through the environment. While the vehicle is moving, this process can be repeated as necessary to generate updated occupancy grids, height maps, and cost maps, so that the path-planning algorithm can recalculate paths appropriately.

[0067] Here we describe further examples for path planning and vehicle control.

[0068] Specifically, presented below is an autonomous navigation system for vehicles (such as ground robots) traversing harsh, unstructured terrain through a cohesive arrangement of mapping, deliberate planning, and reactive behavior modules, which allows the system to understand terrain slope, visibility, and vehicle orientation, thereby enabling the robot or other vehicle to perceive, plan, and respond around unsurveyed areas and overcome recessed obstacles, slopes, steps, overhangs, and narrow passages.

[0069] In this example, the virtual surface concept is used to estimate best-case slopes within view-blocked areas of the environment, so that these best-case slopes can be used when estimating traversal costs using the robot's footprint. By updating the virtual surface in real time while continuously planning and collecting data, concave obstacles can be safely approached and avoided if they are found to be unsafe.

[0070] In one example, the method employs a 3D probabilistic voxel occupancy map, which is used in a hybrid AI system that uses vehicle footprints to estimate and constrain the roll and pitch of the vehicle on a planned path, as well as ray tracing for virtual surface architecture suitable for real-time robotic navigation and associated datasets for evaluation. * It also uses a fast algorithm-based planner, which also includes constraints for approaching virtual surfaces (i.e., unsurveyed areas) from optimal angles. Field trial results from urban and cave environments are also presented, demonstrating the ability to navigate concave obstacles in extreme terrain.

[0071] methodology A. Sensing and SLAM Payloads The sensing payload used in the experiments described herein consists of a tilted Velodyne VLP-16 LiDAR on a Microstrain CV5-25 IMU, a rotating encoder track mount, and a custom timing board used for time synchronization between the sensors. This 0.5 Hz rotating LiDAR mount improves sensor coverage around the vehicle, and the tilt angle improves visibility of the ground ahead of the vehicle as well as LiDAR coverage density and diversity. The payload includes an onboard computer, consisting of a Jetson Xavier, used to run custom SLAM software described in (NPL 10). The sensing payload is positioned near the front of the vehicle, ensuring a downward field of view greater than the maximum slope angle the vehicle can traverse.

[0072] The sensing payload exposes the raw LiDAR point cloud as an odometry and Robot Operating System (ROS) data stream. The pack also performs localization of points within the point cloud to account for encoder rotation and LiDAR orientation. As a result, these points are exposed at approximately 290k points per second in the vehicle frame according to the environment, with local odometry pose updated at 100Hz and higher accuracy poses generated in the data stream at approximately 4Hz.

[0073] B. Mapping and Height Map Generation The height map used to generate the base cost map is extracted from a 3D probabilistic voxel occupancy map, which is generated from a combination of 3D LiDAR points and the local odometry output of a SLAM solution as described in

[11] .

[0074] Each ray is embedded in the occupancy map using a technique supporting Gaussian distribution transformations, such as those described in

[12] , and these calculations are performed on a GPU. Each voxel is classified as either occupied or free, with unobserved voxels being the default. In one example, the map is generated with a voxel resolution of 10 cm.

[0075] Because the occupancy map is generated from local SLAM odometry, the map is susceptible to global misalignment errors; that is, the map does not consider any global optimization, such as that enacted by a loop-closing algorithm. This issue is addressed by maintaining the occupancy map only locally around the vehicle (approximately 10 m x 10 m), where the misalignment errors are small. However, it will be appreciated that this is not essential and that global misalignment correction can be performed instead.

[0076] When generating the height map, the occupancy map is assumed to be aligned perpendicular to the z-axis, and individual voxel columns within the occupancy map are examined. It is during height map generation that an additional voxel classification (virtual surface voxel creation) is performed at the interface between free and unobserved space. Specifically, a virtual surface classification is assigned to free voxels that have unobserved voxels directly below them. Virtual surfaces represent best-case surfaces in areas of the map that are shaded and cannot be properly observed. Virtual surface voxels are used only when there are no candidate occupied voxels within the column search constraints.

[0077] In addition to voxel classification, clearance constraints can also be imposed to ensure that there is a large enough vertical space for a vehicle to pass through, where the constraint ensures that there are enough free or unobserved voxels above each candidate voxel to satisfy the clearance constraint.

[0078] An example of three occupancy grid columns considered for height map generation is shown in side view in Figure 5. Column A depicts the virtual surface generated at the interface between known free voxels and unobserved voxels. Columns B and C show known surfaces, and column C also shows surface candidates rejected due to insufficient clearance.

[0079] Virtual surfaces are the primary input for detecting concave obstacles. A virtual surface is any region of voxels in a height map composed of virtual voxels, as described above. A virtual surface represents an area of ​​observation uncertainty and the best-case slope of that area. Such surfaces often arise from shading effects in sensor observations, but also occur when observation of a real surface is not possible, such as around black bodies and water.

[0080] 6, various view-blocking features are shown as vehicle 200 approaches a downward slope 600. This results in three view-blocking regions, including a first region 601 blocked by the top edge of the slope, a second region 602 and a third region 603 blocked by the front and rear of the vehicle, respectively.

[0081] 6 illustrates two factors that affect the observation of concave obstacles: the downward field of view is much better in front of the vehicle 200 because the sensor is closer to the leading edge of the vehicle's body, and the maximum gradient of a downward slope that the robot can observe depends on the distance between the sensor and the edge of the slope, and the height of the sensor.

[0082] It will be appreciated that there is inherent uncertainty in the use of virtual surfaces, in that the virtual surfaces are an upper boundary to the surface directly below them, and therefore the actual surface may be lower than the virtual surface. However, as the vehicle approaches the edge of the slope, actual observations can be made and an actual surface can be generated in the height map. An example of this is shown in Figures 7A-7C, which show a series of height maps generated as a vehicle containing a sensor approaches a downward slope.

[0083] In this example, the virtual surface is initially generated when the vehicle is positioned far from the edge beyond which it can observe the slope. As the vehicle approaches the edge, the slope of the virtual surface increases until the actual slope is directly observed. As shown in Figure 6, there is a limit to the downward visibility, so if the slope is steeper than the best downward visibility, the virtual surface will continue to increase in slope until it reaches its limit and no actual observation of the slope occurs. The conclusion to be drawn is that the virtual surface cannot be identified as a concave obstacle until the vehicle is very close to the potential surface and has sufficient downward visibility.

[0084] C. Costmap Generation The occupancy grid generates a 2.5D height map that labels each cell as actual (observed), hypothetical (best case guess), or unknown. Within this height map, it is possible to identify obstacles (e.g., hallway walls) that cannot be traversed in any conceivable way. These obstacles are identified and labeled in the cost map.

[0085] Thus, the cost map contains the same information as the height map, but with additional labels for non-fatal (possibly traversable) and fatal (definitely not traversable) cells. Examples of cost maps are shown in Figures 8B and 9B for the scenarios presented in Figures 8A and 9A, respectively.

[0086] 8A and 8B, vehicle 200 is assigned a target destination 821 that is behind a barrier 822. In this scenario, a virtual surface 823 is created by the shadow of barrier 822, which allows for the planning of a path 824 to approach the destination. In this example, green cells 825 indicate observed possibly traversable cells, magenta cells 826 indicate observed critical (non-traversable) cells, and orange cells 823 indicate the virtual surface.

[0087] 9A and 9B, vehicle 200 is assigned a target destination at the base of steep slope 921, the shadow of which creates a virtual surface 923. Again, green cells 925 indicate observed possibly traversable cells, magenta cells 926 indicate observed fatal (non-traversable) cells, and orange cells 923 indicate the virtual surface.

[0088] Virtual surfaces are often treated as if they were actual, observed surfaces. The reason for this is that they represent the best-case slopes (as shallow as possible) that they can contain. If the best-case slopes are traversable, they can only be labeled as non-fatal (possibly traversable). If the best-case slopes are untraversable, they should be labeled as fatal (definitely untraversable). An exception to this is when a virtual cell is caused by the shadow of a convex obstacle, such as in Figure 8A. In this case, the virtual surface should remain non-fatal to allow planning around the corner. For this reason, all virtual cells are labeled as non-fatal. Only the upper ends (higher elevations) of slopes between cells are labeled as fatal. As a result, fatally steep virtual surfaces can only result in fatal cost calculations, where the virtual surface interfaces with actual observations of surfaces higher than them. This is a good description of the upper edges of concave obstacles.

[0089] The current approach uses the GridMap library described in (Non-Patent Document 13) to deploy a series of filters to generate a cost map. Minor modifications to GridMap were made to load parameters differently and perform minor optimizations. The filters used to identify critical obstacles were similar to those used in (Non-Patent Document 14), with the addition of removing small concave surfaces that a vehicle can easily cross, and using vertical sections in three directions instead of four.

[0090] D. Hybrid A * Route planning using The path planning method is a hybrid A method described in (Non-Patent Document 15). * This was implemented using a variation of the algorithm. The method was used to generate kinematically feasible paths that properly handled nearby obstacles and virtual surfaces. The following is a summary of the A method applied to the 3D kinematic state space of the vehicle. * This is an example of a path planner. The algorithm inputs are a cost map, the current vehicle position, and a goal configuration.

number

[0091] Current Hybrid A * The method is a heavily modified implementation based on the technique described in (Non-Patent Document 16). The method dynamically generates a search graph based on a limited set of motion primitives that define the vehicle's motion constraints. The search is performed on a discrete 3D configuration space represented as a grid, while each cell is decomposed to generate a solution that is not aligned with the grid.

number

[0092] The current method employs a custom cost function definition, which considers the unstructured environment and the proposed virtual surface to calculate the neighboring configurations, q i and q j It allows transitions between, which are defined as follows:

number

number

[0093] The most important of these penalties are: Fatal penalty: If the robot's footprint after a transition intersects with any fatal cell in the cost map, the penalty (and transition cost) is +∞. Reverse and Spot Turn Penalty: If the transition ends with the robot relying on the virtual surface for support and the motion primitives involve reverse driving or spot turns, the penalty is +∞. Roll and pitch penalties. Both pose values ​​are estimated using the robot's footprint shape and height map. If their magnitude is less than a threshold, the penalty is 1. The penalty increases with increasing pitch or roll magnitude up to the threshold. If the pitch or roll magnitude exceeds the threshold, the penalty is +∞. If the pitch is negative (the robot faces an uphill slope) and the transition ends with the robot relying on the support of a virtual surface, the pitch and roll are considered to be 0 (horizontal). This is to ignore virtual convex obstacles such as those in Figure 8A.

[0094] This cost function allows us to * can utilize any traversable virtual surface that is useful in reaching the goal. From a distance, virtual surfaces cannot be identified as traversable or non-traversable. By planning as if those virtual surfaces were traversable, the robot approaches virtual surfaces that may provide a useful route. During approach, if the virtual surface contains a fatal concave obstacle, the slope of the virtual surface becomes fatally steep. In this situation, the cost function returns an infinite cost for any plan that enters the virtual surface. Hybrid A is then * generates only routes to the goal that avoid concave obstacles.

[0095] During this process, a local occupancy map is maintained indefinitely within a set distance of the vehicle, such as a radius of approximately 5m, so that if the robot stays within 5m of a concave obstacle, it is remembered.

[0096] E. Behavior Selectors and Decision-Making Policies Ensuring safe traversal between two points is often a non-trivial task for mobile robots, even with sophisticated sensing, mapping, or planning capabilities, especially in real-world scenarios, mainly due to high sensing uncertainty and unpredictability of the surrounding environment the robot may encounter.

[0097] In one example, current approaches use rule-based, heuristically modeled behavior selection techniques similar to finite state machines (FSMs), for example, as described in (Non-Patent Document 17).

[0098] State transitions are based on heuristically defined behavior priorities. For example, if the vehicle determines it has become stuck while attempting to follow a desired trajectory, a collision avoidance behavior is activated. Behavior priorities can be defined empirically through multiple road tests, but priority selection can be adapted to different environments or platforms.

[0099] A set of independent behaviors was used to generate the velocity commands executed by the robot. These behaviors run in parallel, each designed to be activated for a specific task or in a specific situation. Each behavior constantly monitors the current situation and outputs the velocity (acceptability) at which the robot should execute. This behavior system helps the robot cope with unknown and unstructured environments in two main ways. First, if a behavior determines that it cannot perform its task, control is handed over to the next behavior. Second, if a task or situation that existing behaviors cannot handle is encountered, the need for a new behavior can be identified. The addition of a new behavior has minimal impact on the rest of the system, reducing the probability of backtracking during development.

[0100] The system's nominal behavior is path following, tracking a predetermined input path. The behavior receives the vehicle's input path and pose and generates velocity commands not only to follow the path, but also to reactively accelerate or decelerate the robot based on the delta between the current robot pitch and the future desired robot pitch according to the input path section being approached. This effectively tells the vehicle to slow down while approaching a path section that goes downhill and to speed up while approaching a path section that goes uphill, and the vehicle speed is corrected while the vehicle is traversing an inclined path to avoid slipping or over-rolling due to terrain obstacles such as rocks or individual steps of a staircase. Slowing down when approaching a downward slope is important because it allows time for better detection and processing of incremental observations, which can refine the virtual surface or reveal the actual surface below.

[0101] When the robot attempts to move past a certain threshold where it would begin to roll or tip over, path following becomes unacceptable. Under these same conditions, orientation correction behavior becomes acceptable. In this case, control is seamlessly handed over from path following to orientation correction. The orientation correction is performed quickly, based solely on the robot's current orientation, to prevent the robot from tipping or rolling over.

[0102] Most of the behavior is self-explanatory and known in the art, but an overall system diagram of the proposed approach and the data flow between each module is shown in Figure 10. Box 1010 shows the individual modules, and box 1020 shows the behavior (highest priority at the top).

[0103] Experimental results Figures 11A, 11E, and 11I are photographs of the robot used as a test vehicle to test the methodology described herein, and Figures 11B-11D, 11F-11H, and 11J-11L show sample cost maps as it approaches different virtual surfaces.

[0104] 11B-11D show the robot approaching the edge of a rock cliff with a traversable slope nearby. The goal was set to a position beyond the base of the cliff. Initially, the cliff is represented by a traversable virtual surface. Hybrid A * generated a path down a cliff. As the robot approached, the virtual surface became steeper and steeper, eventually becoming untraversable. After this, the robot retreated, and Hybrid A * generated a path that avoided the steepest parts of the cliff.

[0105] Figures 11F-11H show the robot approaching a traversable downward slope. Initially, the downward slope was imaginary. During the approach, the slope was observed and labeled as non-lethal, allowing the robot to traverse it.

[0106] 11J-11L depict the robot approaching a sharp edge of the platform. Again, Hybrid A * initially planned a path down the edge. After approaching the edge, the edge was identified as uncrossable, and the robot backed up and switched to a path that avoided the edge.

[0107] A. Performance Evaluation If the robot failed to avoid the concave obstacle, it could be damaged during the fall. To allow for failure, a Gazebo-based simulation was used. The simulation included a similarity of most of the system's components. Among other things, the simulation included models of the vehicle, sensor configuration, and several environments. The vehicle model was geometrically accurate, but no attempt was made to emulate any dynamic mechanical components. The sensor model included tilting and rotating Velodynes that closely emulated the actual sensor configuration. The accuracy of the geometry and sensor model made the simulation a valuable tool during development and testing. The greatest inaccuracy in the simulation was the environment model. These environment models were polygonal and did not emulate the natural complexity and roughness of real environments.

[0108] Figures 12A and 13A include images of simulated tests of two alternative scenarios. In these examples, vehicle 200 approaches obstacles including a ditch 1200 and a barrier 1300 and a ramp 1301. The resulting cost maps are shown: Figures 12B, 12C, and 13B, 13C show the virtual surface approach; Figures 12D, 12E, and 13D, 13E show the obstacles treated as untraversable; and Figures 12F, 12G, and 13F, 13G show the obstacles treated as traversable. These cost maps show goals 1210, 1310 and calculated paths 1211, 1311.

[0109] As shown in Figures 12F and 12G, when the obstacle is considered traversable, it is unable to handle non-traversable concave obstacles. This is because the vehicle does not have any direct observation of the blocked vertical surface beyond the edge. The concave obstacle was always completely virtual. No matter how close the vehicle approached the edge, it was never able to directly observe the slope of the concave obstacle below it. This led to the robot planning to drive into a deep ditch and falling.

[0110] As shown in Figures 13D and 13E, when the obstacle was deemed untraversable, the planner never attempted to move toward the unclear downward slope. Reaching the goal required a detour down the slope, which was not observable from the robot's starting position. Because the virtual surface above the slope was deemed untraversable, the robot never found a path to the goal. The robot only planned to the nearest position in space in front of the slope, never observing it, and never attempting to traverse it.

[0111] As shown in Figures 12B, 12C and 13B, 13C, both cases could be handled when the virtual surface was considered the best-case slope, regardless of what lay beneath it. In Figure 12B, the vehicle approached the edge and identified the virtual surface as representing an untraversable concave obstacle. In Figure 13B, the vehicle planned to travel on the virtual surface above the slope, move toward the slope, observe the slope, and then plan to travel down the slope toward the goal.

[0112] Thus, the approach described above provides a simple yet efficient technique for generating best-case surface predictions in areas of poor sensor observation. These predictions, designated as virtual surfaces, can be effectively costed in path planning to generate valid paths that first approach and then avoid such poorly observed areas. The above example also demonstrates how the presence of virtual surfaces allows for the inference of concave obstacles and how these surfaces change as they are approached. Practical testing builds confidence that the approach can handle a variety of challenging terrains.

[0113] Throughout this specification and the claims that follow, unless the context otherwise requires, the word "comprise" and variations thereof ("comprises" or "comprising") will be understood to imply the inclusion of a stated integer or group of integers or steps, but not the exclusion of any other integer or group of integers. As used herein, unless otherwise specified, the term "about" means ±20%.

[0114] It will be understood that many variations and modifications will become apparent to those skilled in the art, and all such variations and modifications that become apparent to those skilled in the art should be considered to fall within the spirit and scope of the invention as broadly viewed before being set forth.

Claims

1. 1. A method for use in navigating a vehicle in an environment, comprising: a) acquiring distance data from a distance sensor mounted on the vehicle while the vehicle traverses the environment, the distance data being indicative of a distance between the distance sensor and a sensed portion of the environment; b) analyzing the distance data; i) mapping data indicative of a three-dimensional map of the environment; and ii) position data indicating the position of one or more range sensors within the environment; and c) using the mapping data and the location data to identify obstructed portions of the environment; and d) generating a virtual surface based on occluded portions of the environment; and e) calculating a navigable path within the environment using at least part of the virtual surface; f) controlling the vehicle to move the vehicle within the environment using at least part of the navigable path; g) updating the virtual surface using acquired distance data while the vehicle is moving within the environment; h) recalculating the navigable path using, at least in part, the updated virtual surface; and Including, method.

2. and, in the one or more electronic processing devices, reducing vehicle speed while the vehicle is approaching the virtual surface. The method of claim 1.

3. In the one or more electronic processing devices: a) using the mapping data to generate a three-dimensional occupancy grid representing locations of sensed portions of the environment within a three-dimensional volume; b) using the occupancy grid to identify view-blocked portions of the environment; further comprising:

3. The method according to claim 1 or 2.

4. In the one or more electronic processing devices: a) updating the occupancy grid using acquired distance data while the vehicle is moving within the environment; b) recalculating the navigable path using, at least in part, the updated occupancy grid; and further comprising: The method of claim 3.

5. and further comprising: at the one or more electronic processing devices, identifying view-obstructed portions of the environment using a projection between the distance sensor locations and corresponding sensed portions of the environment. The method according to any one of claims 1 to 4.

6. In the one or more electronic processing devices: a) populating an occupancy grid with occupied voxels using sensed portions of the environment; b) populating the occupancy grid with free voxels based on the projection; c) for each vertical column of the occupancy grid, populating the occupancy grid with virtual surface voxels; further comprising for each vertical column of the occupancy grid, populating the occupancy grid with the virtual surface voxels; i) the free voxels are positioned above unobserved voxels; ii) there are no observed voxels below the free voxel; The method of claim 5.

7. In the one or more electronic processing devices: a) identifying the virtual surface based on adjacent virtual surface voxels; b) calculating the navigable path using the virtual surface; and further comprising: The method of claim 6.

8. In the one or more electronic processing devices: a) calculating the gradient of said virtual surface; b) determining whether the virtual surface is traversable based on the slope; further comprising: The method according to any one of claims 1 to 7.

9. and recalculating, at the one or more electronic processing devices, the virtual surface based on updates to an occupancy grid while the vehicle is moving within the environment. The method according to any one of claims 1 to 8.

10. In the one or more electronic processing devices: a) determining the clearance of the vehicle; b) calculating the navigable path at least in part using the clearance of the vehicle; further comprising: The method according to any one of claims 1 to 9.

11. In the one or more electronic processing devices: a) comparing the vehicle clearance with the height of successive free voxels for a column of an occupancy grid; b) determining whether the column is traversable based on the results of the comparison; and further comprising: The method of claim 10.

12. In the one or more electronic processing devices: a) generating a height map indicative of surface heights within the environment using the populated occupancy grid; b) calculating the navigable path using the height map; and further comprising: The method according to any one of claims 1 to 11.

13. In the one or more electronic processing devices: a) generating a cost map indicating costs associated with traversals of the environment; b) calculating the navigable path using the cost map; and further comprising: The method according to any one of claims 1 to 12.

14. and generating, at the one or more electronic processing devices, the cost map using the height map by labeling non-traversable obstacles in the height map. The method of claim 13.

15. 1. A system for use in navigating a vehicle within an environment, comprising: a) acquiring distance data from a distance sensor mounted on the vehicle while the vehicle traverses the environment, the distance data being indicative of a distance between the distance sensor and a sensed portion of the environment; b) analyzing the distance data; i) mapping data indicative of a three-dimensional map of the environment; and ii) position data indicating the position of one or more range sensors within the environment; and c) using the mapping data and the location data to identify obstructed portions of the environment; and d) generating a virtual surface based on occluded portions of the environment; and e) calculating a navigable path within the environment using at least part of the virtual surface; f) controlling the vehicle to move the vehicle within the environment using at least part of the navigable path; g) updating the virtual surface using acquired distance data while the vehicle is moving within the environment; h) recalculating the navigable path using, at least in part, the updated virtual surface; and one or more electronic processing devices configured to: system.

16. 1. A computer program product for use in navigating a vehicle in an environment, the computer program product, when executed by one or more suitable programmed electronic processing devices, causing the one or more processing devices to: a) acquiring distance data from a distance sensor mounted on the vehicle while the vehicle traverses the environment, the distance data being indicative of a distance between the distance sensor and a sensed portion of the environment; b) analyzing the distance data; i) mapping data indicative of a three-dimensional map of the environment; and ii) position data indicating the position of one or more range sensors within the environment; and c) using the mapping data and the location data to identify obstructed portions of the environment; and d) generating a virtual surface based on occluded portions of the environment; and e) calculating a navigable path within the environment using at least part of the virtual surface; f) controlling the vehicle to move the vehicle within the environment using at least part of the navigable path; g) updating the virtual surface using acquired distance data while the vehicle is moving within the environment; h) recalculating the navigable path using, at least in part, the updated virtual surface; and comprising computer executable code for causing Computer program products.

Images