Route Planning

Abstract

A system for generating a cost map for use in planning a path through an environment, the system including one or more suitably programmed processing devices configured to: obtain a map of the environment; obtain traversal data indicative of an ability to traverse portions of the environment; calculate, for each portion of the environment, a traversal indicator indicative of an ability to traverse that portion; the traversal indicators being calculated in a color space and using the traversal indicators to colorize the map, thereby generating a colorized cost map indicative of an ability to traverse the environment; and the colorized cost map allowing the path planning to be performed.

Description

Technical Field

[0001] The present invention relates to a method and apparatus for generating a cost map used when planning a route through an environment, and in particular, in one example, to the generation of a colored cost map that can be used for route planning. The present invention also relates to a method and apparatus for route planning, and in one example, to route planning using a colored cost map.

Background Art

[0002] Any reference in this specification to any conventional publication (or information derived therefrom) or to any thing known is not an admission, acknowledgement or any form of suggestion that the conventional publication (or information derived therefrom) or the thing known forms part of the common general knowledge in the technical field of the research to which this specification relates and should not be so construed.

[0003] Route planning is an essential element of an autonomous vehicle. This defines the optimal (or nearly optimal) route that the vehicle should follow to move from a starting point to a final goal according to any criteria. In practice, the task is usually realized by considering two main elements, namely, global route planning and local route planning. The global plan provides a complete and overall route based on a known map M of the environment under the assumption that the world is static. As the vehicle moves through the global route, it often encounters moving obstacles or changes in the world that were not originally built into the map M (identified by its on-board scanner). The local planner ideally avoids such obstacles and adapts the vehicle's route, thereby causing the vehicle to deviate from its original plan but attempting to return to the global route as soon as the environmental situation permits.

[0004] Most conventional path planning algorithms are suitable for scenarios where the cost of travel is independent of the direction of travel. In other words, a path planned to travel from point A to point B is identical to a path planned to travel from point B to point A.

[0005] However, in most practical applications, discriminating the direction of movement is key to successful operation. For example, in urban networks, there are several rules regarding lane direction on both one-way and two-way roads, and failure to comply with these rules can lead to accidents. The problem also exists in 3D off-road terrain, where, for example, a vehicle can move downhill but cannot move uphill on a given slope, and vice versa. In industrial areas or plants, there are often traffic rules for machinery (e.g., forklifts, trucks) and pedestrian areas.

[0006] Due to traffic characteristics, directionality in route planning is crucial for autonomous vehicles. Directional constraints in such scenarios are typically encoded as a controlled graph representation with edge weights, and the optimal route is found by running minimum-cost route search on such road networks. At the street level, road lane graphs are used to guide autonomous vehicles regarding the direction of motion. These lane graphs are partially algorithmically generated from relatively high-level street network maps and partially edited by humans. These methods quickly become uncontrollable in semi-structured and unstructured environments.

[0007] For unstructured, coarse terrain, the most common approach is to generate a costly grid map, run the A* or Dijkstra algorithm on the global path map, and then run a local planner based on a relatively sophisticated model for online fitting. [Overview of the Initiative] [Problems that the invention aims to solve]

[0008] Often, due to the simplification of realistic constraints and the abstraction of the problem, the prescribed global path generates a path that the local planner cannot execute. One example is terrain affected by directional constraints. For instance, vehicle control and traction may only allow the vehicle to move in one direction, thereby preventing it from moving in other directions from the same location or causing it to lose traction. Global navigation functions can encode directionality within the map. Often, this requires formulating cost or value functions on the map, which are then used to generate navigation policies at each location. However, since the contributing factor to cost (the effect of traction on mobility) is not always measurable and has coupled nonlinear effects, it is difficult to derive such a cost function.

[0009] U.S. Patent Application Publication No. 20150142251 describes a vehicle control based on colors representing navigation information. In one embodiment, the method includes receiving an image of an object surface having a plurality of colors on its surface that represent data to be used when navigating a vehicle along a route. Furthermore, the method includes controlling the vehicle to act in order to navigate the route based on the image. [Means for solving the problem]

[0010] In a broader embodiment, one aspect of the present invention aims to provide a system for generating a cost map used when planning a route through an environment, the system comprising one or more appropriately programmed processing units configured to acquire a map of the environment, acquire cross-sectional data indicating the ability to traverse parts of the environment, calculate a cross-sectional indicator for each part of the environment indicating the ability to traverse that part, the cross-sectional indicator being calculated in a color space, colorize the map using the cross-sectional indicator, thereby generating a colorized cost map indicating the ability to traverse the environment, the colorized cost map allowing the route planning to be performed.

[0011] In one embodiment, the colorized cost map is colorized to indicate the permissible direction of movement in different parts of the environment.

[0012] In one embodiment, the colorized cost map is colorized such that different allowed directions of movement are indicated by different colors.

[0013] In one embodiment, the color space is at least one of a hue / saturation color space, a hue / saturation / value color space, and a hue / saturation / lightness color space.

[0014] In one embodiment, the color space is a hue / saturation / value color space, and the colorized cost map is colorized such that directionality is indicated by the hue component, directionality cost is indicated by the saturation component, and obstacle cost is indicated by the value component.

[0015] In one embodiment, cross-sectional data represents one or more paths within an environment, and in this case, one or more processing devices use the cross-sectional data to calculate a path vector at each of several points along the path, the path vector indicating the direction of movement at that point on the path and generated in a color space, and for each part of the environment, the devices are configured to obtain a path vector within the part of the environment from one or more paths and to generate a cross-sectional indicator using the path vector.

[0016] In one embodiment, one or more processing devices are configured to determine the transverse direction at points along the path and to calculate a path vector using the transverse direction by converting the transverse direction into a color space.

[0017] In one embodiment, one or more processing devices are configured to acquire trajectory data indicating the trajectory of a vehicle moving along a path, and to determine the direction of crossing using the trajectory data.

[0018] In one embodiment, one or more processing devices are configured to generate a cross-sectional indicator by combining path vectors.

[0019] In one embodiment, one or more processing devices are configured to generate a cross-sectional indicator by combining path vectors in different color spaces and converting the result of the combination step into a color space.

[0020] In one embodiment, one or more processing devices are configured to combine path vectors within the RGB color space.

[0021] In one embodiment, one or more processing units are configured to generate a cross-sectional indicator by averaging path vectors.

[0022] In one embodiment, one or more processing devices are configured to determine a weighting associated with a route vector and to generate a crossing indicator using the weighted average of the route vectors.

[0023] In one embodiment, one or more processing devices are configured to determine the weighting by spreading the direction of movement at a point on the route.

[0024] In one embodiment, one or more processing devices are configured to obtain crossing data by at least one of analyzing the crossing of one or more vehicles through the environment according to a user input command, receiving data from one or more vehicles through the environment, and receiving data from a mapping system, where the mapping system includes a mapping device that traverses the environment and thereby generates mapping data used to generate a map of the environment.

[0025] In one embodiment, one or more processing devices are configured to determine one or more candidate routes and to display the one or more candidate routes on a colored cost map that allows a visual inspection of the one or more candidate routes.

[0026] In one embodiment, one or more processing devices are configured to determine a start and a goal and to generate one or more candidate routes according to the start and the goal using a cost map.

[0027] In a broad embodiment, one aspect of the present invention seeks to provide a method for generating a cost map used when planning a route through an environment, the method comprising: acquiring a map of the environment in one or more appropriately programmed processing units; acquiring cross-sectional data indicating the ability to traverse parts of the environment; calculating a cross-sectional indicator for each part of the environment, the cross-sectional indicator being calculated in a color space; and colorizing the map using the cross-sectional indicator to generate a colorized cost map indicating the ability to traverse the environment, the colorized cost map allowing a route plan to be performed.

[0028] In a broader embodiment, one aspect of the present invention aims to provide a computer program product that generates a cost map used when planning a route through an environment, the computer program product includes computer executable code such that, when executed by one or more appropriately programmed processing units, the processing units acquire a map of the environment, acquire cross-sectional data indicating the ability to traverse parts of the environment, calculate a cross-sectional indicator for each part of the environment indicating the ability to traverse that part, the cross-sectional indicator is calculated in a color space, and use the cross-sectional indicator to colorize the map, thereby generating a colorized cost map indicating the ability to traverse the environment, the colorized cost map allowing a route plan to be performed.

[0029] In a broader embodiment, one aspect of the present invention aims to provide a system for planning a route through an environment, the system comprising one or more appropriately programmed processing units configured to acquire a colorized cost map showing the traverse capability of the environment, determine one or more candidate routes, and use one or more candidate routes and the colorized cost map to plan a route.

[0030] In one embodiment, one or more processing units are configured to display one or more candidate routes on a colorized cost map to allow for visual inspection of one or more candidate routes.

[0031] In one embodiment, the colorized cost map is colorized to indicate the permitted directions of travel along multiple different paths.

[0032] In one embodiment, the colorized cost map is colorized such that different allowed directions of movement are indicated by different colors.

[0033] In one embodiment, the color space is at least one of a hue / saturation color space, a hue / saturation / value color space, and a hue / saturation / lightness color space.

[0034] In one embodiment, the color space is a hue / saturation / value color space, and the colorized cost map is colorized such that directionality is indicated by hue, the coat associated with directionality is indicated by saturation, and secondary costs are indicated by value.

[0035] In a broader embodiment, one aspect of the present invention seeks to provide a method for planning a route through an environment, the method comprising the steps of: obtaining a colorized cost map showing the traversal capability of the environment in one or more appropriately programmed processing units; determining one or more candidate routes; and using one or more candidate routes and the colorized cost map to plan a route.

[0036] In a broader embodiment, one aspect of the present invention aims to provide a computer program product for planning a route through an environment, the computer program product comprising computer executable code that, when executed by one or more appropriately programmed processing units, causes the processing units to obtain a colorized cost map showing the traversal capability of the environment, determine one or more candidate routes, and use one or more candidate routes and the colorized cost map to plan a route. It should be understood that the broader embodiments of the present invention and their individual features can be used in relation to and / or independently, and references to other broader embodiments are not intended to be limiting. Furthermore, it should be understood that the features of a method can be executed using a system or apparatus, and the features of a system or apparatus can be implemented using a method.

[0037] Hereinafter, various examples and embodiments of the present invention will be described with reference to the attached drawings. [Brief explanation of the drawing]

[0038] [Figure 1] This is a flowchart illustrating an example of the process for generating a cost map used when planning a route through an environment. [Figure 2] This is an example of a cost map. [Figure 3] This is a flowchart illustrating an example of a process that uses a cost map to plan a path through the environment. [Figure 4] This is a schematic diagram of an example of a processing system. [Figure 5A-5B] This is a flowchart of a specific example of the process of generating a cost map used when planning a route through an environment. [Figure 6A] This is a schematic diagram illustrating an example of hue mapping for vehicles moving in different directions. [Figure 6B] Figure 6A is a schematic diagram showing an example of the path colors of vehicles moving in different directions, based on hue mapping. [Figure 6C] This is a schematic diagram of a cost map showing several vehicle routes through the environment. [Figure 7A] This is a schematic diagram of an example of a hue / saturation / value color space. [Figure 7B] This is a schematic diagram of the hue / saturation / value color space mapped to the Cartesian coordinate system used when analyzing cross-sectional data. [Figure 8A] This is a schematic diagram of an example of an environment map showing a sample vehicle trajectory. [Figure 8B] This is a schematic diagram of an example of a colorized route based on the sample vehicle trajectory in Figure 8A. [Figure 8C] This is a schematic diagram of an example of a weighting function used to generate the colorized paths shown in Figure 8B. [Figure 9A] This is a schematic plan view of an example of a 3D point cloud used to generate a colorized cost map. [Figure 9B] Figure 9A is a schematic plan view of an example of a colorized cost map generated from a point cloud. [Figure 9C] This is a schematic plan view of an example route calculated using conventional cost calculation methods. [Figure 9D] This is a schematic plan of an example route calculated using map restrictor cost calculation. [Figure 9E] This is a schematic plan view of an example route calculated using a colorized cost map. [Figure 10A] This is a schematic perspective of an example of a 3D point cloud used to generate a colorized cost map. [Figure 10B] Figure 10A is a schematic plan view of an example of a colorized cost map generated from a point cloud. [Figure 10C] This is a schematic plan view of an example route calculated using conventional cost calculation methods. [Figure 10D] This is a schematic plan of an example route calculated using map restrictor cost calculation. [Figure 10E]This is a schematic plan view of an example route calculated using a colorized cost map. [Figure 11A] This is a schematic perspective of an example of a 3D point cloud used to generate a colorized cost map. [Figure 11B] Figure 11A is a schematic plan view of an example of a colorized cost map generated from a point cloud. [Figure 11C] This is a schematic plan view of an example of the first route calculated using conventional cost calculations. [Figure 11D] This is a schematic plan view of an example of the first route calculated using a colorized cost map. [Figure 11E] This is a schematic plan view of an example of a second route calculated using conventional cost calculations. [Figure 11F] This is a schematic plan view of an example of a second route calculated using a colorized cost map. [Modes for carrying out the invention]

[0039] The following describes an example of how to generate a colorized cost map used when planning a route through an environment, with reference to Figure 1.

[0040] For illustrative purposes, the process is assumed to be performed at least in part using one or more electronic processing units that form part of one or more processing systems, such as a computer system or similar, which may optionally be connected to one or more other devices via a network architecture or similar. For ease of illustration, the remainder of the description will refer to the processing units, but it should be understood that multiple processing units may be used in a manner in which the processing is distributed among the devices as needed, and that singular references encompass plural configurations and vice versa.

[0041] In this example, in step 100, the processing unit obtains a map of the environment. The method by which this is achieved will vary depending on a preferred embodiment and may include, for example, the steps of obtaining a pre-generated map from a database or other repository, and generating a map based on mapping data, a point cloud, or something similar. Such techniques are known in the art and therefore no further detailed explanation will be given. However, it should be noted that the map of the environment is usually a two-dimensional environment map, and examples of such maps are well known in the art.

[0042] In step 110, the processing unit acquires cross-sectional data that demonstrates the ability to traverse a portion of the environment. The characteristics of the cross-sectional data and the manner in which it is acquired will vary depending on the preferred implementation. For example, cross-sectional data may be acquired by having one or more vehicles traverse the environment in a state where the vehicles are autonomously, semi-autonomously guided, or driven by an operator. In this example, the cross-sectional data may be collected by onboard sensors, including GPS sensors, radar, LiDAR, or similar, without limitation, and may represent the vehicle's trajectory through the environment.

[0043] In addition to and / or instead, cross-sectional data may also be captured by static sensors provided within the environment, such as cameras or other imaging devices or similar devices placed within the environment. However, this is not mandatory, and other methods may be used, such as having the user manually sketch possible vehicle trajectories or otherwise input details of parts of the environment that may or may not be crossed. For example, this may include having the operator use their knowledge of the specific area to track the paths that can be driven by a vehicle through the environment, and optionally overlaying any applicable rules, such as road traffic rules or similar.

[0044] In a specific example, cross-sectional data can be derived from mapping data used to generate an environmental map. In this example, the mapping data can be collected using a mapping system such as a range sensor that crosses the environment to collect range data indicating the extent to features within the environment. The range data can then be decomposed into point clouds and trajectories across the environment by using a simultaneous localization and mapping (SLAM) algorithm that allows the range data to be used to generate both maps and cross-sectional data.

[0045] In step 120, the processing unit uses the crossing data to calculate a crossing indicator that shows the ability to cross different parts of the environment. The crossing indicator is calculated in a color space such that the color of the indicator represents the ability to cross that part of the environment. The ability to cross a part of the environment may be a binary value, for example, indicating whether the part of the environment can or cannot be crossed, but more generally, it is graded to reflect restrictions on crossing, for example, to indicate directionality and the degree to which directionality applies. For example, a lane on a motorway or highway may have a high degree of unidirectionality, while a single-lane road may have bidirectionality. Finally, a car park or other similar area may have multidirectionality.

[0046] The areas where indicators are determined are typically small regions, such as a group of one or more pixels in the final cost map, and are usually based on the portion of the path traversed by a vehicle or similar, depending on the characteristics of the cross-sectional data. As a result, performing this process for multiple portions of the environment that can be traversed allows the environment map to be colorized in step 130, thereby generating a colorized cost map that shows the ability to traverse the environment.

[0047] Figure 2 shows an example of a colorized cost map.

[0048] In this example, the cost map shows several different vehicle routes 201 that can be traversed, with the routes colored to represent directional requirements. In this example, the routes are roads colored by hues representing the permitted direction of travel, as shown by the color mapping 210. In this example, the color mapping includes red at 0° (corresponding to north in the map), purple at 60°, blue at 120°, cyan at 180°, green at 240°, and yellow at 300°. Thus, the road portion 201.1 colored red indicates that vehicles are permitted to travel north, while the road portion 201.2 shown in cyan indicates that vehicles can travel south. Therefore, the coloring of any portion of the road indicates the permitted direction of travel along the road.

[0049] Therefore, the above method operates by utilizing information about crossing environments to generate a colorized cost map to represent the ability to cross environments. In this view, it should be understood that the reference to the ability to cross environments may include not only physical limitations on movement through environments, but also any rules or similar applicable to crossing environments.

[0050] Once generated, the colorized cost map can then be used to allow the execution of route planning in step 140. In this regard, such planning can be performed using a combination of autonomous, semi-autonomous, or manual techniques. For example, autonomous calculation can be performed by using color as the cost function, thereby allowing routes to be developed based on minimum cost. However, if human involvement is required to consider routes, for example, these routes can be displayed on the colorized cost map, thereby allowing for easy visual evaluation of the routes, and consequently allowing the operator to make a decision on whether the route is appropriate.

[0051] The following describes some further features.

[0052] As described above, in one example, the colorized cost map is colorized to indicate the permissible direction of movement within different parts of the environment, and specifically, different permissible directions of movement are indicated by different colors. This allows for the use of visual inspection to quickly determine whether a proposed route satisfies directional requirements. Specifically, the direction of the proposed route can be compared to the colors displayed on the colorized cost map to determine whether directional requirements are met. While the following example focuses on directionality, it should be understood that similar methods can be used to indicate other restrictions on crossing through the environment. For example, gradients could be colorized to allow for the use of visual inspection to determine whether a slope can be crossed by a vehicle. In other examples, color information could be used to represent restrictions on speed, velocity, acceleration, or other relevant information.

[0053] In one example, the color space is a hue / saturation color space, and more specifically, a hue / saturation / value (HSV) color space or a hue / saturation / lightness (HSL) color space. From the following explanation, it should be understood that while HSV is particularly useful because it allows for the transmission of different ranges of information, this is not mandatory, and any other suitable color space may be used.

[0054] In one example, when the HSV color space is used, the colorized cost map is colorized such that directionality is indicated by the hue component and directionality cost is indicated by the saturation component. Thus, highly saturated colors mean that directionality must be strictly adhered to, while different directions can be mapped to RGB colors, whereas relatively low saturation means that there is a relatively large degree of flexibility regarding directionality. As will become clear from the following explanation, areas of the map where there is no directionality restriction will typically appear as white as a result of minimum saturation. In addition, the value component can then be used to indicate obstacle costs in the case of no restriction, which has a large value and therefore a white appearance, while impassable obstacles that cannot be traversed will have a black appearance.

[0055] In one example, cross-sectional data represents one or more paths within an environment. In this view, the term "path" should be understood to encompass a single cross-section along a given path within an environment, so that multiple paths may encompass not only multiple paths or multiple vehicles traveling along a single route, but also one or more vehicles traveling along multiple different routes.

[0056] Once a path is identified from the cross-sectional data, the processing unit is configured to use the cross-sectional data to calculate a path vector at each of several points along the path. The path vector indicates the direction of movement at that point on the path and is generated in color space such that the path is effectively transformed through the environment into a sequence of color space path vectors distributed along the length of the path.

[0057] In one example, the processing device can be configured to determine the direction of crossing at points along a route from crossing data, which may involve, for example, the steps of: determining the vehicle trajectory from trajectory data showing the trajectory of a vehicle moving along a route; and then determining the direction of crossing using the trajectory, for example, by extrapolating the direction of movement from the trajectory or by deriving the vehicle's pose from the trajectory data, depending on the characteristics of the data.

[0058] Once the direction of movement is determined, this is then used to calculate the path vector by converting the transverse direction into a color space. This is typically achieved using a mapping that associates different directions with different hue values, and this example will be discussed in more detail later.

[0059] Once path vectors are calculated at multiple points for each path, the processing unit can combine the path vectors from multiple paths within individual parts of the environment, which are then used to generate cross-sectional indicators. Thus, in this example, the path vectors from multiple paths are combined where these paths overlap, making it possible to generate cross-sectional indicators within multiple different parts of the environment, which can then be used collectively to colorize the cost map.

[0060] It should be understood that combining path vectors from different routes allows for a relatively better understanding of directional constraints. For example, on a single-lane road, a single crossing of the road will result in a high degree of directionality being assigned to the road. However, if the road is a single-lane two-way road, i.e., actually two-way, this will only be specified when multiple paths along the road are considered for vehicles moving in different directions. Similarly, on a one-way road, considering multiple paths along the road will prevent vehicles moving in the wrong direction from excessively influencing the directional costs associated with that road.

[0061] Path vectors are typically combined in different color spaces, with cross-indicators being generated by converting the results of the combination step into a color space. Therefore, for example, path vectors are generated in an HSV space that can be combined in an RGB space, allowing for the use of linear combinations of path vectors to generate a meaningful color, where the resulting combined color is converted back into the HSV space.

[0062] Path vectors can be combined in any suitable manner, but in one example, this is done by averaging the path vectors, and more specifically by using a weighted average. In this view, weighting can be used to take into account the fact that directionality is not usually confined to separate points, but rather, more generally, applies to the region around a point. However, as you move further away from a point, the degree of certainty associated with directionality decreases, which can be reflected by using weighting. In practice, this means that path vectors farther away from a point are linked to several path vectors surrounding each point on the path, with a relatively large uncertainty in directionality and therefore a reduced weight.

[0063] In one example, this is achieved by broadening the directions of movement associated with vehicles around points on the path, so that the assumption is made that points adjacent to the point under consideration at the present time also have similar directional requirements. It should be understood that any inaccuracies in this method are usually compensated for by averaging the path vectors from multiple different paths within any given portion of the environment. Furthermore, as will be discussed in more detail later, this allows the directional assumption to propagate to areas of the environment that are not actually traversed, despite a low degree of directional certainty, thereby allowing the path to blend into the background.

[0064] As described above, cross-sectional data can be determined in various ways depending on the preferred implementation. For example, cross-sectional data can be determined according to user input commands, for example, by having the user sketch an acceptable travel path on an environment map. More generally, this is achieved by analyzing the cross-sections of one or more vehicles through the environment, and specifically by receiving data from one or more vehicles moving through the environment in the form of GPS or LiDAR data, for example. In a particular example, this can be achieved by receiving data from a mapping system that includes a mapping device that crosses the environment and thereby generates mapping data used to generate a map of the environment. For example, a vehicle equipped with LiDAR and implementing a simultaneous localization and mapping (SLAM) algorithm provides not only point cloud data that can be used to generate an environment map, but also the trajectory of the mapping device through the environment, which can then be used as cross-sectional data.

[0065] As described above, the system can be configured to allow the execution of route planning, and an example of this process will be explained below with reference to Figure 3.

[0066] In this example, in step 300, the start and end points are determined to represent the beginning and end of the route. The start and end points can be determined in any specific way; in this example, this is achieved by manually defining them, but instead, they could be obtained from pre-stored data or something similar.

[0067] In step 310, the processing unit can generate at least one candidate route. The route can be generated using various route planning methods, such as the map restrictor method, and optionally, as will be described in more detail later, it can also be calculated using a colorized cost map by using color as the cost function. Then, in step 320, optionally, the candidate routes can be displayed to allow the user to perform a visual inspection in step 330, thereby evaluating one or more candidate routes.

[0068] Therefore, this method allows colorized cost maps to be used autonomously and / or semi-autonomously, and in certain cases, allows visual inspection to be used to quickly assess the variability of autonomously derived paths.

[0069] In one example, the processing unit is configured to generate a path using a cost map. By generating a path using a cost map, a relatively optimal path (relatively short / relatively fast / avoiding dangerous obstacles) can be generated. The difference between a standard path and the optimal path generated by the cost map becomes relatively large when the traversed environment contains multiple obstacles that are dangerous or difficult to overcome (in this case, the optimal path may be counterintuitive to the user when initially observing the map of the environment).

[0070] In one example, the processing unit is configured to navigate and / or control a vehicle through the environment according to a route and / or cost map generated using the cost map. This allows the vehicle to drive autonomously through the environment while taking the cost map into consideration, for example, by following a route planned using the cost map, or by dynamically controlling the vehicle using the cost map, for purposes such as avoiding obstacles, ensuring that one-way roads are navigated in the correct direction, or similar purposes.

[0071] However, it should be understood that this is not mandatory, and cost maps can be used to assist individuals driving a vehicle within an environment. In this example, the cost map would be presented on a screen within the vehicle, thereby showing the user where the vehicle can or cannot traverse. Conventional maps of the environment would typically not show this information, especially if there are elevation changes or obstacles that could hinder vehicle traversal. By presenting navigation instructions to the user, the user only needs to focus on the direction of the vehicle, without having to interpret a map or devise a way to traverse the environment.

[0072] In one example, the process is executed by one or more processing systems, and Figure 4 shows an example of a suitable processing system 420.

[0073] In this example, the processing system 420 includes at least one microprocessor 421 interconnected via a bus 425 as shown in the figure, memory 422, optional input / output devices 423 such as a keyboard and / or display, and an external interface 424. In this example, the external interface 424 can be used to connect the processing system 420 to peripheral devices such as a communication network, a database 426, other storage devices, or similar. Although a single external interface 424 is shown, this is for illustrative purposes only, and in practice, multiple interfaces using various methods (e.g., Ethernet, serial, USB, wireless, or similar) can be provided.

[0074] When in use, the microprocessor 421 executes instructions in the form of application software stored in memory 422 to allow the required processes to be executed. The application software may consist of one or more software modules and may be executed within a suitable execution environment, such as an operating system environment or something similar.

[0075] Therefore, it should be understood that the processing system 420 can be formed from any suitable processing system, such as a properly programmed client device, PC, web server, network server, or similar. In a particular example, the processing system 420 is a standard processing system, such as a processing system based on the Intel architecture, which runs a software application stored on non-volatile (e.g., hard disk) storage, although this is not required. However, it should be understood that the processing system may be any electronic processing device, such as a microprocessor, microchip processor, logic gate configuration, FPGA (Field Programmable Gate Array) or other electronic device, system, or configuration, optionally associated with implementation logic and firmware, or any other electronic device, system, or configuration.

[0076] However, it should be understood that the above-mentioned configurations assumed for the purposes of the subsequent examples are not mandatory, and that numerous other configurations may be used. For example, the process can be implemented using a distributed configuration such as a client-server configuration, a cloud-based or edge computing configuration, or something similar, depending on the specific implementation.

[0077] The following will provide a more detailed description of a specific example of the process described above, which involves generating a colorized cost map to encode directionality within the cost map, and then subsequently using it for path planning.

[0078] The following describes a specific example of a processor that generates a cost map, with reference to Figures 5A and 5B.

[0079] In this example, in step 500, the processing system 420 retrieves cross-sectional data from the database 426. The cross-sectional data represents data collected from vehicles moving along a route through the environment and may be based on arbitrary location data such as GPS coordinates, mapping data such as LiDAR data, or similar data. The cross-sectional data typically includes several different routes, which may be routes traversed multiple times by one or more vehicles, or different routes traversed by one or more vehicles.

[0080] In step 505, the processing unit 420 selects the following path for analysis, and then, in step 510, segments the path to select points along the path. The points are typically separated by a predetermined distance, in which case this is used to avoid analyzing the total length of the path and thereby reduce computational requirements.

[0081] In step 515, the processing system 420 determines the direction of movement at each point. The direction of movement can be the orientation or posture of the vehicle (or a sensor mounted on top of it) at each point, if known, and / or it can be derived based on the trajectory traversed by the vehicle. For example, if the trajectory data contains only location information such as a sequence of GPS coordinates or similar, it may be necessary to interpolate these in order to derive the movement path and, therefore, to estimate the orientation of the vehicle.

[0082] In step 520, the processing system 420 operates to broaden the direction of movement in order to effectively "broaden" the direction of movement to the area surrounding the point where the direction of movement has been determined. Since the confidence in the exact direction of movement decreases as one moves away from the point, the direction is broadened in step 525 according to the merge weights, as will be described in more detail later.

[0083] This process is repeated for every point along the route, and when the route is complete, it is determined in step 530 whether all routes are complete. If not, the process returns to step 505 to analyze the next route.

[0084] Once all paths are complete, the processing system operates in step 535 to average the path vectors of a portion of the environment map, such as groups of pixels, within the RGB color space. The averaging is performed according to merge weights, so that vectors further from the actual measurement points are given relatively smaller weights throughout the averaging process. The averaging process is used in step 540 to generate the average vector, which is then converted to the HSV color space in step 545 to form cross-sectional indicators of individual parts of the environment. Once this is done, the cross-sectional indicators are added to the cost map in step 550 to allow the cost map to be colorized.

[0085] The following provides further details regarding the specific characteristics of the preferred implementation.

[0086] As described above, the global planner operates on a known map M. When navigating through map M, the vehicle navigates only through areas that are feasible due to physical, legal, or arbitrary constraints. Based on this inherent characteristic of the way / place the vehicle must travel, the scheme attempts to extract information from the vehicle trajectory (by assuming the vehicle has a localization system that provides such a trajectory) to embed a preferred direction within the map. The main challenge is to effectively represent the direction in a way that can be mathematically interpreted by a route planning algorithm, as well as intuitively understood by a human observer of the map.

[0087] In one example, the map is generated using a SLAM algorithm that simultaneously generates the map and tracks the position of sensors as they move through the environment. Thus, any map M will have a trajectory T associated with it, in this case the trajectory T is the path followed by the sensors when collecting data for mapping. In the case of autonomous operation for larger ground vehicles (e.g., cars, trucks, forklifts, industrial carriers, agricultural machinery, etc.), map generation in SLAM is typically performed by sensors mounted on the vehicle itself (unlike maps generated via, for example, handheld or aerial platforms). Thus, in the mapping process, the vehicle (and consequently the sensors) navigates only through areas that are feasible due to physical, legal, or arbitrary constraints. Based on this inherent characteristic of the way the map is generated, the method can extract information from the trajectory to incorporate preferred orientations within the map used for global planning.

[0088] Next, directionality is represented in a way that can be mathematically interpreted by path planning algorithms, as well as intuitively understood by human observers of the map. Since two-dimensional path planning algorithms for ground vehicles often analyze cost maps represented in image data structures, this method represents directionality as different colors within the map.

[0089] In one example, the path on the cost map is colorized based on the two-dimensional orientation of the sensor during navigation, in which case the orientation can be within the range [0, 2π]. This color encoding is performed within the HSV color space, in which case the hue (H) component also has a circular representation that is within the range [0, 2π]. An example of this is shown in Figures 6A to 6C.

[0090] In this example, Figure 6A shows the HSV hue mapping 630 associated with vehicles 631, 632, and 633 moving in different directions, and in this case, Figure 6B shows the colors of the resulting vehicle paths 634, 635, and 636.

[0091] Figure 6C shows an example of a cost map resulting from the analysis of multiple routes, in which case it shows route 601 colorized according to the direction of vehicle movement along the route. Thus, in this example, hue (H) is used to indicate the direction in which a vehicle can and, therefore, must travel when traversing the route.

[0092] While only hue may be used, more commonly, saturation (S) and value (V) components are also used, in which case the saturation component defines the directional importance in a particular path (for example, there are areas where a vehicle can travel in both directions without any consequences, and other areas where travel in a given manner is absolutely prohibited). In Figure 6C, low directional importance is shown in the "white" path segment 601.1. In these segments, vehicles traverse in multiple directions, thus demonstrating the "bidirectional" nature of the path, resulting in reduced saturation. Finally, the value component represents the overall cost in the map, thereby embedding obstacles (i.e., small values ​​shown in black represent obstacles).

[0093] In one example, a modified A* algorithm is used to generate cost maps, with the mathematical formulation incorporating HSV costs in the optimization, in addition to free versus non-free space costs. This approach adds minimal computation to the map generation process, while potentially increasing the computational complexity of the path planning depending on the number of color channels considered.

[0094] For illustrative purposes, the following example refers to a LiDAR inertial algorithm based on continuous-time SLAM, as it is suitable for the operating environment of an autonomous vehicle. However, this is not mandatory, and it should be understood that this can be relatively broadly applied to any dataset that may be used to derive a feasible vehicle trajectory through the environment.

[0095] In this example, the map restrictor method is used as the basis for generating the cost map. From this perspective, when navigating within a pre-mapped area to perform a given task, it is reasonable to assume that in many cases the most desirable route taken by the vehicle corresponds to a pre-traversed route. This assumption allows low-cost routes to be added to the conventional cost map, prompting the planner to follow the pre-traversed route. This method relies on the generation of a trajectory list in addition to any other requirements for generating the cost map. Biasing the planner towards the pre-traversed route increases the likelihood that an efficient and safe route will be adopted without completely limiting the planner's ability to deviate from the route.

[0096] However, this type of restriction is simple with respect to direction. This simplicity is demonstrated in the experiments shown in Figures 9 and 10, which will be described in more detail later, and shows how the cost map can be modified to give priority to pre-traversed routes. Throughout the following description, this route bias strategy will be referred to as the “map restrictor” scheme.

[0097] A standard cost map is a scalar field representing the cost of traversing each point within the field. Typically, this field is discretized as a two- or three-dimensional grid of values. These values ​​determine the cost of traversing the grid cells, and the path planner aims to minimize the resulting integrated cost along the path length. However, ambiguity can exist in the way these costs are represented.

[0098] For example, the cost C within the range [0,1]⇒{C∈:0≦C≦1} has the drawback that any obstacle can be traversed, no matter how dangerous, if the sum of the costs along the path (including obstacles) is less than the costs for all other paths. Therefore, instead, this method represents costs by a "utility value" between 0 and 1, which is the reciprocal of the cost. This allows obstacles and solid road lines to be, for example, absolutely impassable (infinite cost), and this means that terrain never has a cost of zero, and therefore there is always a unique minimum cost path.

[0099] For many driving tasks, the direction of movement needs to be encoded within the map, for example, so that the planned route does not guide the vehicle into an oncoming vehicle. This requires a vector-field cost map that can be discretized in two dimensions within an image of pixels with vector values. As a result, it is convenient for both color images and their corresponding editing software to use vector maps, in which case the vector space is the color space of the image.

[0100] In this method, the direction "utility value" is calculated from the dot product (.) of the candidate vehicle direction vector between the color vector for each pixel, resulting in a signed value within the range [-1,1]. However, since the utility value is preferably within the range [0,1], a constant 1 is added to the dot product, and the resulting value is divided by 2. The advantage of the dot product is that the resulting cost is linear with respect to the input data. This is appropriate because the color map can be derived from multiple driving paths in the environment, and the linearity ensures that such averaging of color vectors corresponds to the average cost of these vectors.

[0101] This method uses the HSV color space depicted in Figures 7A and 7B, which represent the conventional hue, saturation, and value representations, respectively, and the representation relating the color space to the variables used in this explanation. In this example, hue H represents the intended driving direction, saturation S defines the directional weight, and value V represents the overall cost C of the cell. For a given candidate direction d, the cost is given by the following equation:

number

[0102] This is a cone subset of the vector space, an example of which is shown in Figure 7A, which illustrates a conventional representation with hue, saturation, and value, and in Figure 7B, which relates this color space to the Cartesian coordinate system used in this example.

[0103] In these examples, black represents the highest cost (regardless of the direction of movement), and white represents the lowest cost. Lighter colors represent the lowest cost when moving in the direction corresponding to the hue of the color. This choice fits the driving task because the highest-cost areas (obstacles) are high-cost regardless of the direction of approach, while the lowest-cost areas (roads, open fields, and other vehicle operating areas) may have a suitable direction. In addition to loosely constraining direction with the hue vector, the driving direction can be strictly constrained by using the value component V. As the environment is mapped, obstacles are marked as black (full cost), and all remaining areas are marked as gray (intermediate cost). The driven trajectory is colorized according to the driving direction, and the color is interpolated to a predetermined gray color based on the distance from the trajectory path. This generates a set of routes that are softly constrained to constrain the driving direction.

[0104] In omnidirectional areas such as premises or car parks, the average of multiple driving paths on the same route in different directions produces an averaged white color, as shown in 601.1 in Figure 6C. This is lighter than the gray used as the default for non-obstacle areas, meaning that vehicles will still prefer to drive over areas covered by vehicle crossings without incurring additional directional costs. However, this does not necessarily prohibit vehicles from navigating gray areas, because the system can be parameterized to grant relatively small priority over gray areas only, but not to completely avoid them, in cases where the goal lies within one of those areas.

[0105] Being a conical color space, HSV allows for a direct mapping of directionality (hue), directional certainty (saturation), and obstacles (values) in a format that is interpretable and editable by human operators. The addition of an obstacle layer in the form of a value component allows the planner to be biased towards pre-driven paths rather than being limited to these paths. The use of directional certainty, as well as direction, addresses the ambiguity of intersecting paths and bidirectional movement.

[0106] When generating the cost map, as described above, the vehicle path is segmented into individual points, and in this case, the direction of movement at each point is used to generate a vector. Figure 8A shows an example of this, which shows the vehicle orientation 841 indicating the direction of movement in each of the multiple frames 842 representing individual points on the path.

[0107] To provide the aforementioned smooth transition from a color-saturated path to a default background color and reduced saturation within a bidirectional area, • The breadth of the trajectory, and • Color averaging, These two main processing steps must be completed.

[0108] To generate smooth transitions, a linearly decreasing merge weight α with a radius equal to the maximum distance r is added to each pose in the trajectory T. To generate smooth transitions, the linearly decreasing “merge weight” α, ranging from 1 to 0, reduces overall path saturation. α is parameterized according to the maximum distance r from the center of the trajectory T, as shown in Figure 8C. While a linear reduction of weights with distance is used in this example, it is not mandatory, and other weighting methods such as bell curves or other distributions could be utilized. The direction of movement θ is extracted from the trajectory T, and a color representation vector c is assigned to each point within distance r from the center of the trajectory T. n It is used to calculate [the value]. In this case, it is as follows:

number

number

number

number

number

[0109] Since multiple vectors / pixels can exist, index n is assigned to each to distinguish them. A weighted sum of the vectors generated from T and the background color is calculated to effectively average the list of generated colors and to produce non-saturated colors within the bidirectional area. By averaging the x and y components of the color rather than the direction, the saturation of the final color can be reduced if the summed components act in the opposite direction. In addition, averaging the z component allows for a smooth transition to the background color.

[0110] c Background It is defined as a vector containing the desired background color, and therefore, for example, if the desired background color is gray, we can use the vector (0,0,0.7). In this example, it is as follows:

number

number

[0111] Based on the above formula, the averaged vector for each pixel in the image is, for each pixel in the image, c Average =(x Avg ,y Avg ,z Avg It can be calculated as follows:

number

number

[0112] Next, this vector c Average However, it is converted back to the HSV color space according to the following formula.

number

number

number

[0113] The resulting HSV values ​​are embedded in the cost map used in route planning.

[0114] Therefore, the direction (pause) is extracted from the trajectory before it is expanded, as described in Figure 8C. For each point within this expanded direction (pause), a color vector is calculated and then averaged using the formula described above. The resulting value for each pixel is converted to HSV before being written out for use as a cost map in the plan.

[0115] As described below, several experiments were conducted to test the cost mapping and path planning capabilities.

[0116] Specifically, we present several scenarios comparing the above-mentioned method with standard cost calculation methods, thereby demonstrating the applicability of the method.

[0117] While the method is not limited to any particular type of localization scheme, this example uses LiDAR inertial mapping with a SLAM algorithm. The sensors include a Velodyne PUCK VLP-16 LiDAR and a Microstrain-CV5-IMU. In addition, the LiDAR assembly is mounted on a spinning base tilted at 45° for increased point coverage. The spinning base rotates at approximately 0.5Hz, and LiDAR measurements are streamed at 20Hz. The LiDAR is mounted at a height of 1.88m above the robot frame. The system was implemented in C++ using ROS running on a 2.8GHz LGA1151 CPU and 64GB of RAM.

[0118] To evaluate and demonstrate the differences in behavior between this method, the basic map restrictor method, and conventional cost calculations, three main scenarios were selected. These scenarios include various typical road networks, industrial areas, and off-road environments. The conventional cost calculation scenarios represent a free vs. restricted binary cost map representation.

[0119] Scenario #1, depicted in Figures 9A to 9E, includes a cost map shown in Figure 9B, which has a roadway 901 with lanes and a color mapping 910 used to generate the cost map. The goal point G is located approximately 50 meters from the starting point S. However, the shortest path to the goal proceeds in the opposite direction of movement, according to the site rules. When direction is not important, a short, direct path is preferable, as shown by the paths generated by conventional cost calculation and map restrictor methods, as shown in Figures 9C and 9D, respectively. When direction is important, the paths adopted by this method are considerably longer because they perform a full loop, but they adhere to the encoded directional rules, thus always remaining on the left side of the road, as shown in Figure 9E.

[0120] Scenario #2, shown in Figures 10A to 10E, also includes the cost map shown in Figure 10B, which includes road 1001 and color mapping 1010. The goal point G is located approximately 50 meters from the starting point S. This example shows similar results for both routes adopted by the map restrictor and the conventional cost calculation method, as shown in Figures 10C and 10D, which violate traffic regulations. In contrast, the route adopted by this method, shown in Figure 10E, is significantly longer but complies with these regulations. This scenario also highlights the difference between the map restrictor and the conventional cost calculation, in which case the route adopted by the map restrictor is significantly smoother and more closely follows a pre-driven route, which is a known and plausible path.

[0121] Scenario #3, shown in Figures 11A to 11F, consists of a very steep off-road area. The cost map shown in Figure 11B includes the traversed route 1101 and the color mapping 1110 used to generate the cost map. When considering methods for traversing and ascending a slope, the desirable route is often indirect and follows a relatively gentle uphill slope. Scenario #3 shows both the map restrictor shown in Figure 11D and the route adopted by this method, in which case both follow a safe route encoded by past experience, thereby following a gentle gradient. In contrast, the route adopted by the conventional cost calculation shown in Figure 11C leads the vehicle into the steepest section of the slope, thereby imposing a risk of the vehicle overturning. A secondary characteristic of off-road environments is that routes are often one-way. This characteristic is represented in this case by both the map restrictor and the present method, which allows the vehicle to traverse the slope downwards and along the flat section of the path, as shown in Figure 11F, rather than imposing a risk on the vehicle by moving along the slope, as in the case of the conventional cost calculation in Figure 11E. Note that in all figures showing the trajectory in the experiment, inverted colors are displayed for visualization purposes.

[0122] The computational complexity of this method is divided into two parts. Table 1 shows the computation time required to generate the cost maps needed in various scenarios, while Table 2 shows the time required to compute the resulting trajectories. All results were computed on a 3.4GHz E5-2543 v3 CPU with 126GB of RAM.

[0123] [Table 1]

[0124] [Table 2]

[0125] Table 1 shows that the additional time required for inclusion of colorized routes depends on the map size and the number of track points. This time varies by approximately 1.8 to 3.5 times compared to the time required for non-colorized maps.

[0126] Table 2 shows the time cost of performing additional cost calculation functions in the A* algorithm. However, it is important to note that time cannot be directly compared, but it must be considered in parallel with the trajectory length.

[0127] Therefore, the above method provides an efficient way to embed directionality within a cost map. This method is applicable to map-based navigation and is particularly useful in areas where autonomous navigation is required within a known map. In one example, the method uses past travel directions to associate the directionality in vehicle movement with directions in the HSV color space in order to colorize the cost map, and once the cost map is colorized, it is embedded along with the directionality preference.

[0128] In a specific example, this method generates similarity between a vehicle's orientation angle and its hue angle in the hue-saturation-value color space by using hue, saturation, and value components to encode the orientation, orientation, and scalar cost, respectively, within the cost map image. The cost calculation function can be used by the A* algorithm to embed this information within the route planning algorithm, allowing the route planning to be performed in an autonomous, semi-autonomous, or manual process. The method adds minimal computational complexity to SLAM and provides significant benefits in several practical scenarios. Experiments demonstrate that this scheme allows for proper route planning, while a standard binary cost map fails.

[0129] While the above example focuses on direction, this method can be extrapolated to include any other information from the trajectory in the map, such as speed, acceleration, or any other arbitrary state or behavior of the vehicle.

[0130] Throughout this specification and the appended claims, unless the context requires otherwise, the term “comprise” and variations such as “comprises” or “comprising” should be understood to mean the inclusion of the described complete or group of completes or steps, and not the exclusion of any other complete or group of completes. The term “about” as used herein means ±20%, unless otherwise stated.

[0131] Those skilled in the art will understand that numerous modifications and variations will become apparent. All such modifications and variations that become apparent to those skilled in the art should be considered to fall within the spirit and scope of the invention as extensively described herein.

Claims

1. 1. A system for generating a cost map for use in planning a path for a vehicle through an environment, the system comprising: a) obtaining a map of the environment; b) obtaining traversal data indicative of a capability to traverse the portion of the environment; c) calculating for each portion of the environment a traversal indicator indicative of the capability of the vehicle to traverse that portion, the traversal indicator being calculated in a color space such that the indicator is a color representative of the capability of the vehicle to traverse that portion of the environment; d) coloring the map using the traversal indicators, thereby generating a colored cost map indicative of an ability to traverse the environment, the colored cost map allowing path planning to be performed; one or more processing devices configured to The color space is a hue / saturation / lightness color space, and the colorized cost map is i) the obstacle cost is indicated by the brightness component; ii) directionality is indicated by the hue component; and iii) directional cost is indicated by the saturation component; The system is colorized as follows:

2. The colorized cost map is a) to indicate the directions of permitted movement in different parts of said environment; b) so that different allowed directions of movement are indicated by different colors; 10. The system of claim 1 which is colorized.

3. The color space is a) hue / saturation color space; b) Hue / Saturation / Value color space, and c) Hue / Saturation / Luminance color space; The system according to claim 1 or 2, wherein the system comprises at least one of the following:

4. The traversal data indicates one or more paths within the environment, and the one or more processing devices are configured to: a) using the traverse data to calculate a path vector at each of a plurality of points along a path, the path vector indicating a direction of movement at that point on the path and generated within the color space, the one or more processing devices: i) determining a traversal direction at a point along the route; and ii) calculating the path vector using the traverse direction by transforming the traverse direction into the color space; iii) acquiring trajectory data indicative of a trajectory of the vehicle moving along the route; iv) determining the traversal direction using the trajectory data; and b) for each part of said environment: i) obtaining path vectors within the portion of the environment from one or more paths; and ii) generating a crossing indicator using the path vector; 4. The system according to claim 1, wherein the system is configured so as to:

5. The one or more processing devices include: a) generating a crossing indicator by combining said path vectors; b) combining the path vectors in different color spaces; and c) generating the traversal indicator by transforming the result of the combining step into the color space; 5. The system according to claim 4, configured as follows.

6. The system of claim 5 , wherein the one or more processing devices are configured to combine the path vectors in an RGB color space.

7. 7. A system according to any one of claims 4 to 6, wherein the one or more processing units are configured to generate the crossing indicator by averaging path vectors.

8. The one or more processing devices include: a) determining a weighting associated with said path vector; and b) generating the crossing indicator using a weighted average of the path vectors; 8. The system according to claim 4, wherein the system is configured so as to

9. The system of claim 8 , wherein the one or more processing devices are configured to determine the weighting by spreading out directions of travel at the points on the route.

10. The one or more processing devices include: a) according to a user input command, b) analyzing the trajectory of one or more vehicles through said environment; c) receiving data from one or more vehicles through said environment; d) receiving data from a mapping system, where said mapping system includes a mapping device that traverses said environment and thereby generates mapping data that is used to generate said map of said environment; 10. The system of claim 1, configured to acquire the transverse data in at least one of:

11. The one or more processing devices include: a) determining one or more candidate routes; and b) displaying the one or more candidate paths on the colorized cost map to allow visual inspection by a user of the one or more candidate paths.

11. The system according to claim 1, configured so as to

12. The one or more processing devices include: a) judging the start and finish; and b) generating one or more candidate paths according to the start and goal using the cost map; 12. The system according to claim 1, configured to:

13. The one or more processing devices include: a) generating a path using the cost map; b) autonomously navigating the vehicle through the environment using the path.

13. The system according to any one of claims 1 to 12, configured so as to

14. 1. A method of generating a cost map for use in planning a path for a vehicle through an environment, the method comprising: a) obtaining a map of the environment; b) obtaining traversal data indicative of a capability to traverse the portion of the environment; c) calculating, for each portion of the environment, a traversal indicator indicative of the capability of the vehicle to traverse that portion, the traversal indicator being calculated in a color space such that the indicator is a color representative of the capability of the vehicle to traverse that portion of the environment; d) coloring the map using the traversal indicators, thereby generating a colored cost map indicative of an ability to traverse the environment, the colored cost map allowing path planning to be performed; Including, The color space is a hue / saturation / lightness color space, and the colorized cost map is i) the obstacle cost is indicated by the brightness component; ii) directionality is indicated by the hue component; and iii) directional cost is indicated by the saturation component; The way it is colored.

15. 1. A computer program product for generating a cost map for use in planning a route for a vehicle through an environment, the computer program product, when executed by one or more processing devices, causing the processing devices to: a) obtaining a map of the environment; b) obtaining traversal data indicative of a capability to traverse the portion of the environment; c) calculating for each portion of the environment a traversal indicator indicative of the capability of the vehicle to traverse that portion, the traversal indicator being calculated in a color space such that the indicator is a color representative of the capability of the vehicle to traverse that portion of the environment; d) coloring the map using the traversal indicators, thereby generating a colored cost map indicative of an ability to traverse the environment, the colored cost map allowing path planning to be performed; comprising computer executable code for: The color space is a hue / saturation / lightness color space, and the colorized cost map is i) the obstacle cost is indicated by the brightness component; ii) directionality is indicated by the hue component; and iii) directional cost is indicated by the saturation component; A computer program product that is colorized to resemble a computer program.

Images