Vehicle navigation

The method employs a three-dimensional occupancy grid and optimized path planning to navigate vehicles through narrow gaps by calculating vehicle poses and paths based on motion primitives and terrain, effectively addressing the challenges of navigating narrow spaces in unstructured environments.

WO2024227229A1PCT designated stage expired Publication Date: 2024-11-07COMMONWEALTH SCI & IND RES ORG

Patent Information

Application Number
PCT/AU2024/050429
Authority / Receiving Office
WO · WO
Patent Type
Applications
Current Assignee / Owner
Priority Date
2023-05-04
Filing Date
2024-05-03
Publication Date
2024-11-07

AI Technical Summary

Technical Problem

Existing navigation methods for vehicles, particularly in unstructured environments, face challenges in traversing narrow spaces due to computational limitations and the inability to accurately perceive and plan paths through gaps that are only marginally wider than the robot's footprint, leading to oscillatory behavior and failure to generalize in real-world conditions.

Method used

A method and system that utilize a three-dimensional occupancy grid to calculate vehicle poses and paths through narrow gaps by acquiring mapping data, generating a three-dimensional map, and optimizing vehicle movement based on motion primitives and terrain, while detecting proximity and collisions to avoid obstacles and ensure stable navigation.

Benefits of technology

This approach enables vehicles to accurately navigate through narrow gaps by optimizing vehicle pose and path planning, reducing the likelihood of getting stuck and increasing the range of traversable paths, even in unstructured environments, by using a modified A* algorithm and cost function that considers time, kinematics, and vehicle stability.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure AU2024050429_07112024_PF_FP_ABST
    Figure AU2024050429_07112024_PF_FP_ABST
Patent Text Reader

Abstract

A method for use in navigating a vehicle through a gap within an environment, the method including in one or more electronic processing devices, acquiring mapping data indicative of a three dimensional map of the environment and generating a three dimensional occupancy grid proximate the gap. For at least some positions within the environment, a vehicle pose is calculated based on motion primitives indicative of available vehicle movements and environment terrain determined from the mapping data. Following this, a proximity is detected using the vehicle pose and occupancy grid, with a path through the gap being calculated using the proximity.
Need to check novelty before this filing date? Find Prior Art

Description

VEHICLE NAVIGATION Background of the Invention

[0001] The present invention relates to a system and method for use in navigating a vehicle within an environment, and in one particular example, to a system and method for navigating a vehicle through a gap within an environment. Description of the Prior Art

[0002] The reference in this specification to any prior publication (or information derived from it), or to any matter which is known, is not, and should not be taken as an acknowledgement or admission or any form of suggestion that the prior publication (or information derived from it) or known matter forms part of the common general knowledge in the field of endeavour to which this specification relates.

[0003] Narrow spaces present as challenging obstacles for large mobile robots, and are prevalent in many unstructured environments, such as underground environments, or the like. Planning routes through such narrow spaces is challenging due to the computational limitations of real-time embedded systems. Often low resolution costmaps are used for perceiving terrain to work within these restrictions, however, through coarse maps narrow spaces marginally wider than the robot can be perceived as not traversable. For mission critical applications in challenging conditions, controllers must be generalisable to behave reliably in unstructured environments, which is not the case for existing path planning algorithms.

[0004] Traditional reactive navigation methods, such as Artificial Potential Fields (APF) and bug algorithms, often encounter local minima when approaching narrow spaces where the balance of attractive and repulsive forces result in oscillatory behaviour. Planning methods like Probabilistic Roadmap (PRM) and Rapidly-exploring Random Tree (RRT) rely on constructing feasible paths that consider the kinematics of the robot. These methods are typically unable to generate plans through narrow spaces (marginally wider than the robot), where perception granularity shows the robot footprint as wider than the gap.

[0005] Several methods explicitly incorporate narrow passages into their sampling strategy. For example, a bridge test was introduced in Zheng Sun, D. Hsu, Tingting Jiang, H. Kurniawati, and J. H. Reif, “Narrow passage sampling for probabilistic roadmap planning,” IEEE Transactions on Robotics, 2005, to detect narrow spaces for use with PRM planning. By representing the robot and the environment as a union of ellipsoids S. Ruan, K. L. Poblete, H. Wu, Q. Ma, and G. S. Chirikjian, “Efficient Path Planning in Narrow Passages for Robots with Ellipsoidal Components,” June 2022, arXiv:2104.04658 utilises the representation of robots and the environments as a union of ellipsoids enabling the configuration space obstacles to be parameterised in a closed form and hence enabling efficient path planning in narrow spaces.

[0006] M. Mujahed, D. Fischer, and B. Mertsching, “Admissible gap navigation: A new collision avoidance approach,” Robotics and Autonomous Systems, 2018 develops a virtual admissibility gap detector to create a plan through narrow spaces while satisfying the kinematic constraints of the platform.

[0007] F.-A. Moreno, J. Monroy, J.-R. Ruiz-Sarmiento, C. Galindo, and J. Gonzalez-Jimenez, “Automatic Waypoint Generation to Improve Robot Navigation Through Narrow Spaces,” Sensors, 2020 identify critical navigation points (CNP) to generate auxiliary waypoint locations through narrow regions.

[0008] In work introducing virtual surfaces, T. Hines, K. Stepanas, F. Talbot, I. Sa, J. Lewis, E. Hernandez, N. Kottege, and N. Hudson, “Virtual Surfaces and Attitude Aware Planning and Behaviours for Negative Obstacle Navigation,” arXiv preprint arXiv:2010.16018, Jan. 2021 presents a behaviour stack, where each behaviour has a priority and admissibility criteria. A behaviour that is admissible is activated if it is of higher priority than the current behaviour. The path follow behaviour from this work implements Hybrid A*, an efficient planning algorithm that plans to a goal using a costmap while considering the vehicle constraints.

[0009] Doorways in urban environments often conform to a set of known geometries. R. Rusu, W. Meeussen, S. Chitta, and M. Beetz, “Laser-based perception for door and handle identification,” Advanced Robotics, 2009 develop a geometric detection method that detects a door within a set of dimension specifications from 3D point cloud information. Experiments with a PR2 robot determine the location of a door and its handles, enabling the robot tonegotiate the open or closed configuration to get through. Other methods detect doors by interpreting information from signs above the door.

[0010] However, each of these approaches has difficulty with narrow spaces that may be perceived as smaller than the robot footprint.

[0011] Methods that learn navigation behaviours have demonstrated impressive policies from sensor information directly, and have shown promising results in challenging urban and field environments. However, these methods often generalise poorly in real-world conditions where situations not experienced during training, resulting in unexpected behaviour, as described for example in B. Tidd, A. Cosgun, J. Leitner, and N. Hudson, “Passing Through Narrow Gaps with Deep Reinforcement Learning,” in IEEE International Conference on Intelligent Robots and Systems, 2021.

[0012] K. Rana, V. Dasagi, B. Talbot, M. Milford, and N. Sunderhauf, “Multiplicative Controller Fusion: A Hybrid Navigation Strategy For Deployment in Unknown Environments,” IEEE International Conference on Intelligent Robots and Systems, 2020 reduces the oscillation effects from an APF controller using an ensemble of navigation policies that provide a confidence for running the traditional controller and when to instead use the learned policy.

[0013] Learning methods have demonstrated navigation from RGB image input for outdoor environments in G. Kahn, P. Abbeel, and S. Levine, “BADGR: An Autonomous Self- Supervised Learning-Based Navigation System,” arXiv:2002.05700, 2020, which learns a waypoint navigation policy with a self-supervised approach to control a mobile robot directly from images. This method learns to favour travelling over a concrete path over grass. S. Bansal, V. Tolani, S. Gupta, J. Malik, and C. Tomlin, “Combining Optimal Control and Learning for Visual Navigation in Novel Environments,” arXiv:1903.02531, 2019, train a behaviour to traverse to a goal location through narrow doorways in an indoor office environment.

[0014] S. Gupta, V. Tolani, J. Davidson, S. Levine, R. Sukthankar, and J. Malik, “Cognitive Mapping and Planning for Visual Navigation,” arXiv:1702.03920, 2019, use reinforcement learning to develop an egocentric map and learn to plan to a goal. A. Kumar, S. Gupta, and J.Malik, “Learning Navigation Subroutines from Egocentric Videos,” arXiv:1905.12612, 2019. use expert trajectories to learn motion primitives (turn left, turn right) for traversing corridors and doorways. B. Tidd, A. Cosgun, J. Leitner, and N. Hudson, “Passing Through Narrow Gaps with Deep Reinforcement Learning,” in IEEE International Conference on Intelligent Robots and Systems, 2021, use reinforcement learning to traverse narrow doorways with a large tracked robot from an occupancy map.

[0015] Learning methods are appealing for the development of highly complex behaviours from high resolution perception input, however, these methods often consider a single, well defined navigation scenario, relying on a small terrain set that can be simulated. Unpredictable behaviours emerge when the robot encounters situations not seen during training. For mission critical applications more work is needed for these methods to be applied in the field. Summary of the Present Invention

[0016] In one broad form, an aspect of the present invention seeks to provide a method for use in navigating a vehicle through a gap within an environment, the method including in one or more electronic processing devices: acquiring mapping data indicative of a three dimensional map of the environment; generating a three dimensional occupancy grid proximate the gap; for at least some positions within the environment, calculating a vehicle pose based on: motion primitives indicative of available vehicle movements; and, environment terrain determined from the mapping data; detecting a proximity using the vehicle pose and occupancy grid; and, calculating a path through the gap using the proximity.

[0017] In one embodiment the method includes in the one or more electronic processing devices: populating the occupancy grid with occupied voxels using sensed parts of the environment; and, detecting collisions using the pose of the vehicle and the occupied voxels.

[0018] In one embodiment the method includes in the one or more electronic processing devices, performing the collision detection using a mean of the samples within occupied voxels.

[0019] In one embodiment the method includes in the one or more electronic processing devices: determining a current vehicle position; determining a current pose of the vehicle;assessing one or more neighbouring positions, each neighbouring position being adjacent to the current position, by: determining a neighbouring pose of the vehicle at the neighbouring position; and, performing proximity detection using the neighbouring pose and the occupancy grid; and, using results of the assessing step to calculate the path.

[0020] In one embodiment the method includes in the one or more electronic processing devices, calculating the path by repeatedly: selecting one or more of the neighbouring positions to be a current position; and, assessing one or more neighbouring positions.

[0021] In one embodiment the method includes in the one or more electronic processing devices, calculating the pose using at least one of: motion primitives; and the environment terrain.

[0022] In one embodiment the method includes in the one or more electronic processing devices, optimising the pose.

[0023] In one embodiment the method includes optimising the pose using at least one of: a time taken to traverse to the position; vehicle kinematics; a direction of movement; a vehicle stability for the environment terrain; and, a proximity to occupied voxels.

[0024] In one embodiment the method includes in the one or more electronic processing devices: calculating a cost function for each position; and, using the cost function for multiple positions to calculate a path.

[0025] In one embodiment the cost function is based on at least one of: a time taken to traverse to the position; vehicle kinematics; a direction of movement; a vehicle stability for the environment terrain; and, a proximity to occupied voxels.

[0026] In one embodiment the method includes in the one or more electronic processing devices: optimising the pose; and, calculating the cost function using the optimised pose.

[0027] In one embodiment each position corresponds to a respective cell in the occupancy grid.

[0028] In one embodiment the method includes in the one or more electronic processing devices: generating a graph including a plurality of search nodes interconnected by edges; and, calculating a path by searching the graph using a modified A* algorithm.

[0029] In one embodiment: at least some of the cells contain a node; and, a cell contains at most one node.

[0030] In one embodiment the method includes in the one or more electronic processing devices: acquiring range data from a range sensor mounted on the vehicle as the vehicle traverses the environment, the range data being indicative of a range between the range sensor and a sensed part of the environment; analysing 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; and, using the mapping data to identify a gap.

[0031] In one embodiment the method includes in the one or more electronic processing devices: implementing a narrow space identifier to identify a gap; and, in the event a gap is identified, executing a gap path planning algorithm to: generate the three dimensional occupancy grid proximate the gap; calculate vehicle poses; and, calculate the path.

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

[0033] In one embodiment the method includes in the one or more electronic processing devices, at least one of: reducing a vehicle velocity as the vehicle approaches a gap; and, recalculating the path by: updating the occupancy grid as the vehicle moves; and, recalculating the path using the updated occupancy grid.

[0034] In one broad form, an aspect of the present invention seeks to provide a system for use in navigating a vehicle through a gap within an environment, the system including one or more electronic processing devices: acquiring mapping data indicative of a three dimensional map of the environment; generating a three dimensional occupancy grid proximate the gap; for at least some positions within the environment, calculating a vehicle pose based on: motionprimitives indicative of available vehicle movements; and, environment terrain determined from the mapping data; detecting a proximity using the vehicle pose and occupancy grid; and, calculating a path through the gap using the proximity.

[0035] It will be appreciated that the broad forms of the invention and their respective features can be used in conjunction and / or independently, and reference to separate broad forms is not intended to be limiting. Furthermore, it will be appreciated that features of the method can be performed using the system or apparatus and that features of the system or apparatus can be implemented using the method. Brief Description of the Drawings

[0036] Various examples and embodiments of the present invention will now be described with reference to the accompanying drawings, in which: -

[0037] Figure 1 is a flow chart of an example of a method for use in navigating a vehicle through a gap within an environment;

[0038] Figure 2 is a schematic diagram of an example of a vehicle;

[0039] Figure 3 is a schematic plan view of an example of a vehicle approaching a gap;

[0040] Figures 4A and 4B are a flow chart of a further example of a method for use in navigating a vehicle through a gap within an environment;

[0041] Figure 5 is a flow chart of an example of a method for implementing a narrow gap path planning algorithm; and,

[0042] Figure 6 is an image of an example of a vehicle traversing a gap. Detailed Description of the Preferred Embodiments

[0043] An example of a process for use in navigating a vehicle through a gap within an environment will now be described with reference to Figure 1.

[0044] The environment is typically unstructured, and culd be natural, including an open environment, such as an outdoor area, or confined environment, such as in a cave system or similar. The environment could additionally, and / or alternatively, be a constructed environment, such as a building, underground mine, or the like, or a combination of the natural and constructed environments.

[0045] For the purpose of this example, the vehicle is assumed to be any device capable of traversing an environment, and could include autonomous vehicles, robots, or the like. The vehicle could use a range of different locomotion mechanisms depending on the environment, and could include wheels, tracks, or legs. Accordingly, it will be appreciated that the term vehicle should be interpreted broadly and should not be construed as being limited to any particular type of vehicle.

[0046] Irrespective of the nature of the vehicle, the vehicle will typically include a range sensor, such as a LiDAR sensor, stereoscopic vision system, or the like. Additionally, each vehicle will typically include one or more electronic processing devices configured to receive signals from the range sensor and either process the signals, or provide these to a remote processing device for processing and analysis. In one specific example, this involves implementing a SLAM (Simultaneous Localisation and Mapping) type algorithm to perform simultaneous localisation and mapping. The processing device could be of any suitable form and could include a microprocessor, microchip processor, logic gate configuration, firmware optionally associated with implementing logic such as an FPGA (Field Programmable Gate Array), or any other electronic device, system or arrangement. This process can be performed using multiple processing devices, with processing being distributed between one or more of the devices as needed, so for example some processing could be performed onboard the vehicle, with other processing being performed remotely. Nevertheless, for the purpose of ease of illustration, the following examples will 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 and vice versa, with processing being distributed between the devices as appropriate.

[0047] In this example, the navigation process involves acquiring mapping data indicative of a three dimensional map of the environment at step 100. This can be achieved in any suitable manner, but typically involves acquiring range data from a range sensor mounted on the vehicle as the vehicle traverses the environment. The range data is indicative of a range between the range sensor and a sensed part of the environment, allowing the processing device to analyse 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 is typically indicative of a three dimensional representation of the environment, and may be in the form of a point cloud or similar, with the position data being indicative of a position of the range sensor within the environment, when respective range data is captured. The mapping and position data is generated by processing the range data collected by the range sensor, for example using a SLAM algorithm or similar.

[0048] At step 110, the processing device optionally identifies gaps within the environment using the mapping and optionally position data. In this regard, parts of a three-dimensional volume where the environment is present can be considered as occupied. Ray tracing from a range sensor position to a sensed part of the environment can then be used to identify intervening unoccupied, or "free" parts of the environment, with this in turn allowing gaps to be identified.

[0049] At step 120, the processing device generates a three dimensional occupancy grid. Specifically, this involves generating a three dimensional grid of voxels, also referred to as cells, and populating the voxels based on the location of parts of the environment.

[0050] At step 130, the processing device calculates a pose of the vehicle for at least some positions within the environment. The pose of the vehicle for any given position is generated based on the environment terrain as determined from the mapping data. Typically this is performed to take into account the terrain in the vicinity of the vehicle, and how the vehicle will sit on the terrain. Additionally, this also takes into account motion primitives indicative of available vehicle movements, thereby accounting for how the vehicle will move through the environment, and in turn how this will affect the pose.

[0051] Having calculated the pose for the vehicle, this information is used together with the occupancy grid to allow a proximity of the vehicle to the environment to be determined at step 140. Thus, projecting the shape of the vehicle onto the occupancy grid for the calculated pose, can allow a distance between surfaces of the vehicle and the environment to be calculated, and hence allow the proximity of the vehicle to the environment to be determined.

[0052] Steps 130 and 140 are repeated for multiple positions, which in one example may correspond to different cells within the occupancy grid. This is typically achieved by selecting neighbouring positions, such as neighbouring cells, and assessing the proximity of the vehicle for the neighbouring positions, repeating this progressively for neighbouring positions heading towards a target on the other side of the gap.

[0053] This allows the processing device to progressively assess the proximity of the vehicle to the environment as the vehicle moves through the environment towards a target beyond the gap, and hence allows a path to be calculated at step 150 where proximity to the environment is optimised, for example maximising separation between the vehicle and the environment, to thereby avoid collisions.

[0054] Thus, in the above approach, the use of the three dimensional occupancy grid, as well as calculating the pose of the vehicle taking into account both the terrain and movement of the vehicle through the environment, results in a highly accurate planning approach, which in turn allows the vehicle to more effectively navigate through narrow gaps.

[0055] In one particular example, this approach is performed using a planning algorithm, such as an A* algorithm, in which is a cost function is used to represent movement of the vehicle between search nodes corresponding to cells in the occupancy grid, taking into account and optionally optimising the pose of the vehicle, to thereby maintain a sufficient spacing between the vehicle and the environment.

[0056] Once the path has been created, this can be used to control the vehicle, so that the vehicle navigates within the environment at step 160. In this regard, it will be appreciated that as the vehicle continues to move within the environment, additional range data is acquired, andthis can be used to refine the above process, for example, updating the mapping data and hence occupancy grid, as more of the environment is sensed.

[0057] The above approach provides for greater flexibility in path planning, and in particular avoids gaps in the environment being assessed as non-traversable, which would otherwise limit the ability of path planning to be successfully performed. This enables a greater range of traversable paths to be calculated, and consequently reduces the computation, time and manoeuvring required to successfully traverse an environment. Additionally, this significantly increases the likelihood of a traversable path being correctly identified, increasing the ability of vehicles to traverse unstructured environments, and reducing the potential for vehicles become stuck within an environment.

[0058] An example of a vehicle is shown in more detail in Figure 2.

[0059] In this example, the vehicle 200 includes a chassis and body 210 having at least one electronic processing device 211 located on-board, which is coupled to a mapping system 212 configured to perform scans of the environment surrounding the vehicle in order to build up a 3D map (i.e. point cloud) of the environment. In one example, the mapping system includes a 3D LiDAR sensor such as a VLP-163D LiDAR produced by Velodyne. The 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 allow movement of the vehicle to be controlled, and one or more other sensors 215. This could include proximity sensors for additional safety control, or an imaging device, or similar, to allow images of the environment to be captured, for example, for the purpose of colourising point cloud representations of the environment.

[0060] The processing device 211 can also be connected to an external interface 216, such a wireless interface, to allow wireless communications with other vehicles, for example via one or more communications networks, such as a mobile communications network, 4G or 5G network, WiFi network, or via direct point-to-point connections, such as Bluetooth, or the like.

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

[0062] It will be appreciated that the above described configuration assumed for the purpose of the following examples is not essential, and numerous other configurations may be used. For example, although the vehicle is shown as a wheeled vehicle in this instance, it will be appreciated that this is not essential, and a wide variety of vehicles and locomotion systems could be used.

[0063] An example of a gap will now be described with reference to Figure 3.

[0064] In this example, a gap 311 is shown. In this example, as the vehicle 200 approaches the gap, the location of the gap can be identified based on the ability of the sensor of the mapping system 212 to detect the environment 312 beyond the gap.

[0065] A number of further features will now be described.

[0066] In one example, the above described approach uses a three-dimensional occupancy grid that is generated by the processing device using the mapping data. The occupancy grid represents a location of sensed parts of the environment within a three dimensional volume, and typically includes a three-dimension grid of voxels, with each voxel being labelled based on whether or not the voxel is coincident with a detected part of the environment, for example, by labelling voxels aligned with the environment as occupied.

[0067] Once the occupancy grid has been generated, this can be used in detecting a proximity of the vehicle to the environment, by projecting a known shape of the vehicle onto the occupancy grid based on the pose of the vehicle, and then calculating the proximity using a mean of samples for occupied voxels.

[0068] In one example, the method of calculating a path involves expanding a potential path progressively by examining the effect of moving the vehicle as it moves within the environment. In this example, the processing device operates to determine a current pose ofthe vehicle at a current position within the environment. Once this has been completed, the processing device assesses a number of neighbouring positions within the environment. Specifically, the neighbouring positions are positions to which the vehicle can traverse from the current position, and in one specific example, each position corresponds to a cell within the occupancy grid, so that the processing device assesses the movement and pose of the vehicle as it moves between neighbouring cells, as will be described in more detail below. The positions can be selected arbitrarily and may include, for example, selecting each cell that is adjacent to a current cell containing the current position.

[0069] Each neighbouring position is assessed by calculating a neighbouring pose, and then performing proximity detection using the neighbouring pose and the occupancy grid, with results of this assessment being used to calculate the path. For example, the path can be calculated by progressively assessing neighbouring positions moving away from the current position, selecting one of these neighbouring position to be a next current position and repeating the assessment of one or more neighbouring positions for the newly selected current position. Once the proximity for multiple positions has been assessed, the path can be calculated, for example by minimising a proximity so as to maximise a separation to the environment, and / or ensuring a minimum separation is maintained at all times.

[0070] Typically the neighbouring pose is calculated based on the current pose, motion primitives indicative of allowable movements of the vehicle, and the environment terrain. Thus, it will be appreciated that multiple different neighbouring poses can be established for each neighbouring position depending on how the vehicle is moved to the position, and / or movements of the vehicle at the position. Thus, for example, the ultimate pose of the vehicle will depend not only on the terrain, but also on the direction of movement into the position, whether the vehicle rotates at the position, or the like. Accordingly, in one example, the process involves optimising the neighbouring pose at one or more of the neighbouring positions, with this being performed taking into account factors such as a time taken to traverse to the neighbouring pose, vehicle kinematics, a direction of movement, a vehicle stability for the environment terrain, a proximity to occupied voxels, or the like. Thus, the optimisation can be performed to minimise travel time, maintain or increase a separation relative to the environment, ensure vehicle stability, or the like.

[0071] In one example, selection of a neighbouring position for inclusion in the path is performed by calculating a cost function for each position and then using the cost function for multiple positions to calculate the path. Thus, cost positions for multiple sequential positions defining a path can be summed, with the lowest sum cost being used to define the path. The cost function is typically based on factors such as a time taken to traverse to the position, vehicle kinematics, a direction of movement, a vehicle stability for the environment terrain, a proximity to occupied voxels or the like.

[0072] In one specific example, the above described process is performed using a graph searching approach. In this example a graph is generated including a plurality of search nodes interconnected by edges, with the path being calculated by searching the graph using a modified A* algorithm. The graph is typically generated based on the occupancy grid, so that at least some of the cells contain a node and a cell contains at most one node.

[0073] The above described gap path planning process therefore uses a three dimensional occupancy grid, together with calculation and / or optimisation of vehicle pose at each of a number of cells within the grid. As a result, the algorithm tends to be quite computationally expensive. Accordingly, rather than simply use this planning approach all the time when traversing the environment, the processing device will typically use a more traditional path planning approach when there are no gaps that need to be traversed. Thus, while traversing the environment, the processing device will acquire range data from a range sensor mounted on the vehicle, analysing the range data to generate the mapping data and position data, and then use this to perform path planning. During this process, the processing device will analyse the mapping data to identify if a gap exists, for example using a narrow space identifier, and if so, will invoke the above described path planning process to plan a path through the gap. Otherwise, a more traditional path planning approach is used.

[0074] Once the path has been calculated, the processing device can control the vehicle, causing it to follow the path. As this occurs, the control process can include controlling the vehicle speed, for example, reducing the speed of the vehicle as the gap is approached, which can mitigate any damage arising in the event that an impact with the environment occurs. Additionally, the processing device can use data acquired as the vehicle approaches the gap toupdate the occupancy grid, for example as additional mapping data beyond the gap is captured, allowing the path to be recalculated as needed.

[0075] A further example of a path planning process will now be described in greater detail with reference to Figures 4A and 4B.

[0076] In this example, at step 400 range data is acquired, with mapping and position data being generated at step 405, in a manner substantially similar to that described above. At step 410, a three dimensional occupancy grid is generated, with the processing device populating the grid with occupied voxels based on the location of sensed parts of the environment.

[0077] Next, at step 415, the processing device selects a current cell, corresponding to a cell where the vehicle is located and calculates a pose of the vehicle at step 420. The processing device then selects one or more neighbouring cells at step 425, and calculates a neighbouring pose for each of these selected neighbouring cells at step 430, based on the terrain and motion primitives representing available movements for the vehicle. The processing device then uses the neighbouring pose to project the vehicle extent onto the occupancy grid and then performs proximity detection for each selected neighbouring cell at step 435, using the extent of the vehicle and the nearest occupied cell to determine a separation between the vehicle and the environment, given the current vehicle pose.

[0078] At step 440 the processing device calculates a cost function for each selected neighbouring cell, using the associated neighbouring pose. The cost function is calculated based on the time taken to traverse to the selected neighbouring cell under consideration, from the current cell, as well as other factors, such as vehicle kinematics, a direction of movement (is movement towards or away from a target beyond the gap), a vehicle stability for the environment terrain and the proximity to occupied voxels.

[0079] At step 445, the processing device can optionally optimise the neighbouring pose for one or more of the selected neighbouring cells. This can involve altering the neighbouring pose in order to try and reduce the value of the cost function. For example, this can involve changing the pose of the vehicle to try and reduce the proximity to the environment, or minimise the time taken to reach the pose. Once the optimisation has been performed, theproximity detection and cost calculation can be repeated by returning to steps 435 and 440. This process of optimising the neighbouring pose for one or more of the selected neighbouring cells can be performed in any manner, and may be performed any number of times depending on the preferred implementation. Typically this optimisation is performed a single time for each selected neighbouring cell in order to minimise computation, whilst still providing an opportunity to improve the pose for each selected neighbouring cell.

[0080] Once optimisation has been completed, then a next current cell is selected at step 450, typically using the selected neighbouring cell having the lowest associated cost function as the next current cell.

[0081] Once this has been done, the processing device assesses if the gap has been traversed at step 455, and if so, the path is generated at step 460. Otherwise, the process returns to step 425, allowing further neighbouring cells next to the new current cell to be selected, and the process repeated.

[0082] Accordingly, it will be appreciated that the above described approach constructs the path progressively by examining neighbouring cells, and optimising the pose of the vehicle for selected neighbouring cells, so that an overall low cost path is identified, which passes through the gap, whilst avoiding contact between the vehicle and the environment.

[0083] As the approach requires an analysis of a three dimensional occupancy grid and typically employs at least one iteration of optimisation for each cell under consideration, it is computational expensive. Accordingly, the system typically employs a narrow space detection process to select when to implement the gap path planning algorithm.

[0084] An example of this will now be described in greater detail with reference to Figure 5.

[0085] In this example, at step 500 range data is acquired, with mapping and position data being generated at step 505, in a manner substantially similar to that described above. At step 510, a narrow space detection process is performed, with a view to identify narrow spaces in the environment. If a narrow space is detected at step 515, the gap path planning algorithm is implemented at step 525, otherwise a more traditional algorithm is used at step 520.

[0086] A specific example process will now be described in further detail. A. Regular Planner

[0087] In this section, the traditional planner that is used in the system is described in order to contrast the developments specifically made for the gaps planner algorithm.

[0088] The regular planner, detailed in T. Hines, K. Stepanas, F. Talbot, I. Sa, J. Lewis, E. Hernandez, N. Kottege, and N. Hudson, “Virtual Surfaces and Attitude Aware Planning and Behaviours for Negative Obstacle Navigation,” arXiv preprint arXiv:2010.16018, Jan. 2021, uses hybrid A* on a grid 0.1m pixels in the horizontal plan, and 30º yaw. Obstacles are determined based on a height map generated with 0.1m resolution from a voxelised representation, referred to as an occupancy height map (OHM). An occupancy grid classifies pixels as traversable, virtual surfaces (potentially traversable but the underlying surface has not been directly observed), fatal, and unknown.

[0089] Several difficulties arise from this pipeline when considering narrow spaces. For example, the height map may lose information, and not accurately identify the narrowest point of a gap. Additionally, the hybrid A* planner uses discrete 2D grid based collision tests, and can only describe features on the same scale as its resolution, and so may not accurately capture collisions, without an extremely high resolution, which would be unduly computationally expensive to solve.

[0090] Furthermore, the hybrid A* planner generates a graph by stepping from the start pose using a set of motion primitives. This means that the poses that the search reaches are a finite set determined by the start pose and repeated applications of the motion primitives. When planning through a narrow space the goal should be to plan along the centre of it. How close hybrid A* gets to the centre is dependent on the size of the motion primitive library. The time taken to complete the search is also dependent on the number of motion primitives. The size limit of the motion primitive library that is necessary to get acceptable performance makes it unlikely that the planner will plan along the centre of a narrow space.B. Narrow Spaces Planner

[0091] This narrow spaces planner was developed as an alternative to the occupancy high map (OHM), cost map and hybrid A* system. The narrow spaces planner uses a full 3D voxel occupancy map and does collision checks based on the mean of the samples within occupied voxels. The narrow spaces planner uses something like motion primitives to find connectivity between search nodes but then sets the actual pose of the search node according to the contents of the map and other factors like the robot kinematics and the predecessor node. This means that the poses a plan can contain are influenced by the contents of the map and can be made to be close to the centres of narrow spaces.

[0092] In broad terms, the approach of using the narrow spaces planner involves: ^ a method of detecting narrow spaces in order to determine when the narrow spaces planner should run; ^ constructing a 3D voxel occupancy map that tracks the mean of samples within each occupied voxel; ^ optimising poses based on the pose, the occupancy map and the pose that the robot would reach this pose from; and, ^ a variant of A* that applies the optimisation methods during the search and reuses the same search node poses in order to incrementally optimise them. 1) Representation

[0093] The continuous state consisting of 3D spatial coordinates alongside yaw is denoted as x = (x, y, z, θ) ∈ Х. The discretisation of this space is denoted as N, where xn, n ∈ N is the continuous state representative associated with the n-th discretised state. The continuous cell associated with the n-th discretised state is denoted as Xn, so that xn ∈ Xn. The set of edges is denoted E ⊂ N × N, and G = (N, E). For (n′, n) ∈ E, c(xn′ , xn) denotes the cost of traversing from node n′ to node n. The set of nodes neighbouring node n is denoted by N(n).

[0094] Each cell of the grid contains only one search node, i.e., xn ∈ Xn, and Xn ∩ ∀ n = n′. Combined with the height map, each 4D state xnimplies a pitch and roll. The continuous states xnare initialised to the mean value within the cell. The search grid is reused when searches are repeated in the same region of space.

[0095] Function notation (e.g., H(x)) is used to denote quantities that are computed, and subscript notation (e.g., fn) is used to denote quantities that are stored / updated in arrays for each state.

[0096] The search is a variation of A*, and thus it utilises a priority queue Q ⊂ N sorted by increasing sum fn= gn+ hn, where gnis the cost so far for node n ∈ N of the graph, and hnis the heuristic for node n, evaluated using the continuous heuristic H(xn). Each iteration of the search involves removing the element n∗ with lowest fnfrom the queue (n∗ = arg minn∈Qfn), processing it, and possibly adding new elements into the queue.

[0097] For each n ∈ Q, we have the continuous pose xn, the values fn, gnand hn, along with the predecessor state πn∈ N , and the node state sn∈ {NULL, OPEN, OPTIMISED, CLOSED}. 2) Algorithm:

[0098] The algorithm is roughly: 1) Initially, all search nodes have state NULL. 2) The initial node n0(that contains the start pose) is set to state OPTIMISED, the pose is set to the start pose, F, G and H are updated. 3) The initial node is pushed into the queue. 4) The element with lowest F is removed from the queue as the current node: 5) If the current node is OPEN: a) The current node is optimised, its state is set to OPTIMISED, its F, G and H are updated. b) The current node is pushed into the queue. 6) If the current node is OPTIMISED: a) The current node’s state is set to CLOSED. b) If the current node’s H is the lowest found so far, store it. c) For all neighbour nodes, i) If the neighbour node is NULL: A) The neighbour node’s state is set to OPEN, its f, g and h are updated, and its predecessor is set to the current node. B) The neighbour node is pushed into the queue.ii) Otherwise update f, g and π if the current node provides a better path: 7) Return to 4.

[0099] The node with the lowest H value is used as the end of the plan and the rest of the plan is extracted by stepping backwards through the predecessors. 3) F, G and H:

[0100] G is the cost so far. It is based on the time taken to move from the predecessor to the successor then multiplied by various penalty factors. The time taken is the minimum of (yaw change / max angular velocity, position change / max linear velocity). Some of the penalties are based on: • Would the robot be stably supported by occupied voxels? • How feasible is the movement considering the vehicle kinematics? • Is the movement forwards or backwards? • How close are occupied voxels to the sides of the robot?

[0101] H is the heuristic. 4) Neighbours:

[0102] The neighbours are the 80 cells that are within one x, y, z or yaw step. 5) Optimisation:

[0103] The optimisation is performed in steps. Each execution of the complete search algorithm involves performing at most one optimisation step on each search node. The search nodes can therefore be optimised by as many steps as the number of searches the search grid is preserved for.

[0104] The optimisation considers the following factors: • The vehicle kinematics and making the pose more feasible to reach from its predecessor. • How the robot is likely to be supported by the occupied voxels below it - it is moved within the allowable XYZ and yaw range and to whatever pitch and roll it would be likely to fall to.• How close occupied voxels are to the sides - it is moved away from obstacles and into the centre of narrow spaces. 6) When should it run?:

[0105] The narrow spaces planner fits in to a broader system as one of any number of possible path planners. The outer system expects each planner to generate plans and then picks one of those plans to use. This planner is not as fast or as well tuned as others except when it comes to the narrow spaces it was designed for. Therefore, a narrow spaces detector was created to determine when the narrow spaces planner should try to plan. When no narrow spaces are detected, the planner should be idle and use as little computer power as possible.

[0106] The narrow spaces detector performs two basic A* searches on the existing 2D cost map. One search is with a footprint small enough to always fit through spaces that the robot might even accounting for the worst cases of discretisation error. The other search is with a footprint slightly larger that will fit through spaces that the other hybrid A* planner can handle. When the smaller footprint makes significantly more progress toward the goal than the larger footprint, the narrow spaces planner is enabled. C. Path Execution

[0107] Path execution involves generating command velocities which when sent to the robot platform, enable it to follow the given path. This velocity generation is calculated through the use of short horizon trajectory generation in the Path Follow Behaviour.

[0108] The Path Follow Behaviour generates trajectories from the robot base link to a local goal pose using 10th order Bezier curves. Trajectory generation is optimised such that the form of the trajectory fits the input path as close as possible whilst also adhering to the kinematic constraints of the robot and minimising trajectory completion time.

[0109] The Path Follow Behaviour makes use of a dynamic short horizon envelope which reaches out from the base link of the robot platform out to the nearest obstacle, defined as at least one fatal cell in the cost map. The horizon defines the pose of the local goal, used as the target for all trajectory generation. The local goal is located on the local path at its intersectionwith the horizon. The horizon shrinks and grows as the robot moves closer to or further from obstacles and as it does, Path Follow dynamically adjusts the scaling of the robot kinematic limits thus slowing the robot as it approaches obstacles. This slowing around obstacles allows for more precise path tracking around obstacles, and is essential for narrow gap navigation. A key component of Path Follow is the generation of smooth and continuous trajectories and velocity commands. Newly generated trajectories are generated with specifically set control nodes such that the initial state of the trajectory matches the last robot command. This ensures a smooth continuous kinematic command profile sent to the robot platform whilst constantly regenerating trajectories and is essential for accurate path tracking.

[0110] Active trajectory regeneration is attempted at a rate of 25Hz. If a new active trajectory is generated, robot commands are set as R = T (∆t) where R is the generated set of robot velocity commands in a twist format and T is the trajectory function with t time as input. If a new trajectory cannot be generated which fits current kinematic constraints the existing active trajectory is iterated and robot commands set as R = T (t + ∆t). In the rare occurrence that trajectory regeneration fails continuously and a full trajectory is executed, a safety mechanism is built into every generated trajectory to ensure a safe stopping procedure is commanded at the end of each trajectory.

[0111] Path Follow works in conjunction with the rest of the behaviour stack and as such has been tuned to be quite aggressive. Path Follow will attempt to follow any given path as best it can regardless of whether the route of the path may result in collision with obstacles. Instead Path Follow will merely slow the robot down to a minimum speed, continue to follow the path and rely on other recovery behaviours such as the Decollide and Orientation Correction behaviours to recover the robot from any potentially dangerous collision state.

[0112] The above approach was verified with experiments in simulation and with a real platform negotiating narrow spaces. The platform used for all experiments was the BIA5 all- terrain robot (ATR). The width of the platform is 78 cm, tested with a constructed wooden door that has a frame of 80 cm. The frame is large enough for the robot to traverse, though small enough that typical planning methods usually fail to generate a path. The robot is controlled with heading and forward velocity commands. The robot has a perception system mountedtoward the front of the robot. The perception system performs SLAM and generates a costmap. Further detail is described in N. Hudson, F. Talbot, M. Cox, J. Williams, T. Hines, A. Pitt, B. Wood, D. Frousheger, K. L. Surdo, T. Molnar, R. Steindl, M. Wildie, I. Sa, N. Kottege, K. Stepanas, E. Hernandez, G. Catt, W. Docherty, B. Tidd, B. Tam, S. Murrell, M. Bessell, L. Hanson, L. Tychsen-Smith, H. Suzuki, L. Overs, F. Kendoul, G. Wagner, D. Palmer, P. Milani, M. O’Brien, S. Jiang, S. Chen, and R. C. Arkin, “Heterogeneous Ground and Air Platforms, Homogeneous Sensing: Team CSIRO Data61’s Approach to the DARPA Subterranean Challenge,” submitted to the DRC Finals Special Issue of the Journal of Field Robotics, 2021.

[0113] The metrics used to evaluate the traversal of the door are: • Success: The robot is able to completely traverse the gap, the robot body is sufficiently clear of the frame. • Time(s): The time taken in each attempt is recorded. • Operator Actions: The number of interventions from a human operator. Under poor communications and with increased operator load (for example from coordinating multiple agents) the robot must traverse gaps with as few operator instructions as possible.

[0114] Several methods were compared that showcase the complexity of the problem, and the challenges with current approaches: • Path Planner: The primary A* search path planning method used by the behaviour stack discussed in T. Hines, K. Stepanas, F. Talbot, I. Sa, J. Lewis, E. Hernandez, N. Kottege, and N. Hudson, “Virtual Surfaces and Attitude Aware Planning and Behaviours for Negative Obstacle Navigation,” arXiv preprint arXiv:2010.16018, Jan.2021.. • RL Gap Behaviour: A gap behaviour that was developed using reinforcement learning. Proximal Policy Optimisation (PPO) was used to train a policy for passing through narrow gaps. Full details are found in B. Tidd, A. Cosgun, J. Leitner, and N. Hudson, “Passing Through Narrow Gaps with Deep Reinforcement Learning,” in IEEE International Conference on Intelligent Robots and Systems, 2021. • RL Behaviour Selection: The behaviour selection module trained with reinforcement learning that determines when the gap behaviour should be activated, or when instead to use the behaviour stack. Full details are found in B. Tidd, A. Cosgun, J. Leitner, andN. Hudson, “Passing Through Narrow Gaps with Deep Reinforcement Learning,” in IEEE International Conference on Intelligent Robots and Systems, 2021. • Gap Planner: The above described method for planning through narrow spaces introduced in Section III of this paper. • Autonomous Operation: Autonomously selected the gap planner to reduce operator actions (Section III-B6).

[0115] The simulation environment from the Gazebo simulator is shown in Figure 6, in which a robot 600 attempts to pass through gap 611. The results in Table I demonstrate the difficulty of the task, with the path planner method unable to generate a solution, timing out in both the simulation and real experiments. The reinforcement learning methods perform well in simulation, achieving 93% success with the gap behaviour and 63% success with behaviour selection. However, performance significantly decreases in real world experiments (73% and 40%), limiting the application of RL methods in mission critical scenarios in the real world. Table 1

[0116] Accordingly, it is apparent the gap planner described herein performs the best, and hence represents an improvement over the art.

[0117] Throughout this specification and claims which follow, unless the context requires otherwise, the word “comprise”, and variations such as “comprises” or “comprising”, will be understood to imply the inclusion of a stated integer or group of integers or steps but not theexclusion of any other integer or group of integers. As used herein and unless otherwise stated, the term "approximately" means ± 20%.

[0118] Persons skilled in the art will appreciate that numerous variations and modifications will become apparent. All such variations and modifications which become apparent to persons skilled in the art, should be considered to fall within the spirit and scope that the invention broadly appearing before described.

Claims

THE CLAIMS DEFINING THE INVENTION ARE AS FOLLOWS: 1) A method for use in navigating a vehicle through a gap within an environment, the method including in one or more electronic processing devices: a) acquiring mapping data indicative of a three dimensional map of the environment; b) generating a three dimensional occupancy grid proximate the gap; c) for at least some positions within the environment, calculating a vehicle pose based on: i) motion primitives indicative of available vehicle movements; and, ii) environment terrain determined from the mapping data; d) detecting a proximity using the vehicle pose and occupancy grid; and, e) calculating a path through the gap using the proximity. 2) A method according to claim 1, wherein the method includes in the one or more electronic processing devices: a) populating the occupancy grid with occupied voxels using sensed parts of the environment; and, b) detecting collisions using the pose of the vehicle and the occupied voxels. 3) A method according to claim 2, wherein the method includes in the one or more electronic processing devices, performing the collision detection using a mean of the samples within occupied voxels. 4) A method according to any one of the claims 1 to 3, wherein the method includes in the one or more electronic processing devices: a) determining a current vehicle position; b) determining a current pose of the vehicle; c) assessing one or more neighbouring positions, each neighbouring position being adjacent to the current position, by: i) determining a neighbouring pose of the vehicle at the neighbouring position; and, ii) performing proximity detection using the neighbouring pose and the occupancy grid; and, d) using results of the assessing step to calculate the path. 5) A method according to claim 4, wherein the method includes in the one or more electronic processing devices, calculating the path by repeatedly: a) selecting one or more of the neighbouring positions to be a current position; and,b) assessing one or more neighbouring positions. 6) A method according to any one of the claims 1 to 5, wherein the method includes in the one or more electronic processing devices, calculating the pose using at least one of: a) motion primitives; and b) the environment terrain. 7) A method according to any one of the claims 1 to 6, wherein the method includes in the one or more electronic processing devices, optimising the pose. 8) A method according to claim 7, wherein the method includes optimising the pose using at least one of: a) a time taken to traverse to the position; b) vehicle kinematics; c) a direction of movement; d) a vehicle stability for the environment terrain; and, e) a proximity to occupied voxels. 9) A method according to any one of the claims 1 to 8, wherein the method includes in the one or more electronic processing devices: a) calculating a cost function for each position; and, b) using the cost function for multiple positions to calculate a path. 10) A method according to claim 9, wherein the cost function is based on at least one of: a) a time taken to traverse to the position; b) vehicle kinematics; c) a direction of movement; d) a vehicle stability for the environment terrain; and, e) a proximity to occupied voxels. 11) A method according to claim 9 or claim 10, wherein the method includes in the one or more electronic processing devices: a) optimising the pose; and, b) calculating the cost function using the optimised pose. 12) A method according to any one of the claims 1 to 11, wherein each position corresponds to a respective cell in the occupancy grid.13) A method according to any one of the claims 1 to 12, wherein the method includes in the one or more electronic processing devices: a) generating a graph including a plurality of search nodes interconnected by edges; and, b) calculating a path by searching the graph using a modified A* algorithm. 14) A method according to claim 13, wherein: a) at least some of the cells contain a node; and, b) a cell contains at most one node. 15) A method according to any one of the claims 1 to 14, wherein the method includes in the one or more electronic processing devices: a) acquiring range data from a range sensor mounted on the vehicle as the vehicle traverses the environment, the range data being indicative of a range between the range sensor and a sensed part of the environment; b) analysing the range data to generate: i) mapping data indicative of a three dimensional map of the environment; and, ii) position data indicative of one or more range sensor positions within the environment; and, c) using the mapping data to identify a gap. 16) A method according to any one of the claims 1 to 15, wherein the method includes in the one or more electronic processing devices: a) implementing a narrow space identifier to identify a gap; and, b) in the event a gap is identified, executing a gap path planning algorithm to: i) generate the three dimensional occupancy grid proximate the gap; ii) calculate vehicle poses; and, iii) calculate the path. 17) A method according to any one of the claims 1 to 16, wherein the method includes in the one or more electronic processing devices, controlling the vehicle at least in part using the navigable path to move the vehicle within the environment. 18) A method according to claim 17, wherein the method includes in the one or more electronic processing devices, at least one of: a) reducing a vehicle velocity as the vehicle approaches a gap; and, b) recalculating the path by:i) updating the occupancy grid as the vehicle moves; and, ii) recalculating the path using the updated occupancy grid. 19) A system for use in navigating a vehicle through a gap within an environment, the system including one or more electronic processing devices: a) acquiring mapping data indicative of a three dimensional map of the environment; b) generating a three dimensional occupancy grid proximate the gap; c) for at least some positions within the environment, calculating a vehicle pose based on: i) motion primitives indicative of available vehicle movements; and, ii) environment terrain determined from the mapping data; d) detecting a proximity using the vehicle pose and occupancy grid; and, e) calculating a path through the gap using the proximity.

Citation Information

Patent Citations

  • Path planning

    US20230095898A1

  • Path planning method and path planning apparatus

    WO2022056770A1

Cited By

  • Complex terrain path planning method and system for hybrid prospecting robot

    CN122281931A

  • A method for the operation of a mining or construction machine

    WO2026135515A1