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 methods and apparatus for generating cost maps for use in planning a path through an environment, and in one particular example, to generating colorized cost maps that can be used for path planning. The present invention also relates to methods and apparatus for path planning, and in one particular example, to path planning using colorized cost maps. [Background technology]

[0002] Reference herein to any prior publication (or information derived therefrom) or to any known matter is not, and should not be construed as, an admission or endorsement, or any form of suggestion, that the prior publication (or information derived therefrom) or known matter forms part of the common general knowledge in the art of the study to which this specification pertains.

[0003] Path planning is an essential element of an autonomous vehicle. It defines the optimal (or near-optimal) path that a vehicle should follow to travel from a starting point to a final goal according to any criteria. In practice, the task is usually realized by considering two main elements: global path planning and local path planning. Global planning provides a complete and overall path based on a known map M of the environment, with the assumption that the world is static. As the vehicle travels through the global path, 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 tries to avoid such obstacles and adapt the vehicle's path, thereby causing the vehicle to deviate from its original plan, but return to the global path as soon as environmental conditions allow.

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

[0005] However, in most practical applications, discriminating the direction of movement is key for successful operation. For example, in urban networks, there are several rules regarding the direction of lanes on both one-way and two-way roads, and non-compliance with these rules can lead to accidents. Problems also exist in 3D off-road terrains, for example, where a vehicle can travel downhill but not uphill with a given slope, and vice versa. In industrial areas or plants, there are often traffic rules for machines (e.g., forklifts, trucks) and pedestrian areas.

[0006] Due to the nature of traffic, directionality in path planning is important for autonomous vehicles. Directional constraints in such scenarios are usually encoded as controlled graph representations with edge weights, and optimal paths are found by running a minimum-cost path search on such road networks. At a street-level scale, road lane graphs are used to guide autonomous vehicles with respect to directionality of motion. These lane graphs are partly algorithmically generated from relatively high-level street network maps, and partly edited by humans. These approaches quickly become uncontrollable in semi-structured and unstructured environments.

[0007] For unstructured, coarse terrain, the most common approach is to generate a costed grid map, run A* or Dijkstra's algorithm on the global path map, and run a more sophisticated model-based local planner for on-line adaptation. Summary of the Invention [Problem to be solved by the invention]

[0008] Often, due to simplification of real constraints and abstraction of the problem, the prescribed global path produces a path that the local planner cannot execute. One example is terrain subject to directional constraints. As an example, the control and traction of the vehicle only allows the vehicle to move in one direction, which could result in it getting stuck or losing traction in other directions from the same location. The global navigation function can encode the directionality into a map. Often, this requires formulating cost or value functions on the map, which are then used to generate navigation policies at each location. However, such cost functions are difficult to produce, since the contributors to cost (the effect of traction on mobility) are not always measurable and have coupled nonlinear effects.

[0009] US Patent Publication No. 20150142251 describes vehicle control based on colors representing navigation information. According to one aspect, the method includes receiving an image of an object surface having a plurality of colors thereon representing data used in navigating the vehicle along a path. Further, the method includes controlling the vehicle to operate to navigate the path based on the image. [Means for solving the problem]

[0010] In one broad form, one aspect of the invention seeks to provide 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 parts of the environment; calculate, for each part of the environment, a traversal indicator indicative of an ability to traverse that part; the traversal indicators being calculated in a color space; and colorize the map using the traversal indicators, thereby generating a colorized cost map indicative of the ability to traverse the environment, the colorized cost map allowing the path planning to be performed.

[0011] In one embodiment, the colored cost map is colored to indicate the directions of allowed movement in different parts of the environment.

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

[0013] In one embodiment, the color space is hue / Saturation Color space, hue / Saturation / brightness Color space and hue / Saturation / brightness At least one of the color spaces.

[0014] In one embodiment, the color space is hue / Saturation / brightness In a color space, the colorized cost map has a directionality represented by the hue component and a directional cost represented by Saturation The components are represented by and the obstacle cost is brightness Colored as indicated by ingredients.

[0015] In one embodiment, the traversal data indicates one or more paths within the environment, and wherein the one or more processing devices use the traversal data to calculate a path vector at each of a plurality of points along the path, the path vector indicating a direction of movement at that point on the path and generated in color space, and are configured to, for each portion of the environment, obtain a path vector within the portion of the environment from the one or more paths and generate a traversal indicator using the path vector.

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

[0017] In one embodiment, the one or more processing devices are configured to obtain trajectory data indicative of a trajectory of a vehicle moving along a route, and to determine the traversal direction using the trajectory data.

[0018] In one embodiment, the one or more processing devices are configured to generate the crossing indicator by combining the path vectors.

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

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

[0021] In one embodiment, the one or more processing devices are configured to generate the crossing indicator by averaging the path vectors.

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

[0023] In one embodiment, the one or more processing devices are configured to determine the weightings by spreading out directions of movement at points on the route.

[0024] In one embodiment, the one or more processing devices are configured to obtain the trajectory data by at least one of: analyzing the trajectory of one or more vehicles through the environment in accordance with user input commands; by receiving data from one or more vehicles through the environment; and by 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, the one or more processing devices are configured to determine one or more candidate paths and display the one or more candidate paths on a cost map that is colored to allow visual inspection of the one or more candidate paths.

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

[0027] In one broad form, an aspect of the invention seeks to provide a method of generating a cost map for use in planning a path through an environment, the method comprising the steps of obtaining, in one or more suitably programmed processing devices, a map of the environment; obtaining traversal data indicative of an ability to traverse parts of the environment; calculating, for each part of the environment, a traversal indicator indicative of an ability to traverse that part, the traversal indicators being calculated in a color space; and colorizing the map using the traversal indicators to thereby generate a colorized cost map indicative of an ability to traverse the environment, the colorized cost map allowing the path planning to be performed.

[0028] In one broad form, one aspect of the invention seeks to provide a computer program product for generating a cost map for use in planning a path through an environment, the computer program product including computer executable code that, when executed by one or more suitably programmed processing devices, causes the processing device to obtain a map of the environment; obtain traversal data indicative of an ability to traverse parts of the environment; calculate, for each part of the environment, a traversal indicator indicative of an ability to traverse that part, the traversal indicators being calculated in a color space; and color the map using the traversal indicators, thereby generating a colorized cost map indicative of an ability to traverse the environment, the colorized cost map allowing the path planning to be performed.

[0029] In one broad form, one aspect of the invention seeks to provide a system for planning a path through an environment, the system including one or more suitably programmed processing devices configured to obtain a colorized cost map indicative of traversal capability of the environment, determine one or more candidate paths, and use the one or more candidate paths and the colorized cost map to plan the path.

[0030] In one embodiment, the one or more processing devices are configured to display the one or more candidate paths on a cost map that is colorized to allow visual inspection of the one or more candidate paths.

[0031] In one embodiment, the colored cost map is colored to indicate the directions of allowed movement along different paths.

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

[0033] In one embodiment, the color space is hue / Saturation Color space, hue / Saturation / brightness Color space and hue / Saturation / brightness At least one of the color spaces.

[0034] In one embodiment, the color space is hue / Saturation / brightness In a color space, the colorized cost map indicates the directionality by the hue, and the cost associated with the directionality is calculated by the color space. vinegar To Saturation and the secondary costs are brightness The images are colored as shown by

[0035] In one broad form, one aspect of the invention seeks to provide a method for planning a path through an environment, the method comprising the steps of obtaining, within one or more appropriately programmed processing devices, a colorized cost map indicative of traversal capability of the environment, determining one or more candidate paths, and using the one or more candidate paths and the colorized cost map to plan the path.

[0036] In one broad form, one aspect of the invention seeks to provide a computer program product for planning a path through an environment, the computer program product including computer executable code that, when executed by one or more appropriately programmed processing devices, causes the processing device to obtain a colorized cost map indicative of traversal capability of the environment, determine one or more candidate paths, and use the one or more candidate paths and the colorized cost map to plan a path. It will be appreciated that the broad form of the invention and its individual features can be used in conjunction and / or independently, and references to separate broad forms are not intended to be limiting. Furthermore, it will be appreciated that method features can be performed using a system or device, and system or device features can be implemented using a method.

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

[0038] [Figure 1] 1 is a flowchart of an example process for generating a cost map for use in planning a path through an environment. [Diagram 2] 1 is an example of a cost map. [Diagram 3] 1 is a flowchart of an example process for using a cost map to plan a path through an environment. [Figure 4] 1 is a schematic diagram of an example of a processing system. [Figure 5A-5B] 1 is a flowchart of a particular example of a process for generating a cost map for use in planning a path through an environment. [Figure 6A] FIG. 2 is a schematic diagram of an example of hue mapping for a vehicle moving in different directions. [Figure 6B] FIG. 6B is a schematic diagram of an example of route colors for vehicles moving in different directions based on the hue mapping of FIG. 6A. [Figure 6C] FIG. 2 is a schematic diagram of a cost map showing several vehicle paths through an environment. [Figure 7A] FIG. 1 is a schematic diagram of an example of a hue / saturation / lightness color space. [Figure 7B] FIG. 1 is a schematic diagram of the hue / saturation / lightness color space mapped to the Cartesian coordinate system used in analyzing the cross-sectional data. [Figure 8A] FIG. 2 is a schematic diagram of an example of an environment map showing sample vehicle trajectories. [Figure 8B] FIG. 8B is a schematic diagram of an example of a colorized path based on the sample vehicle trajectory of FIG. 8A. [Figure 8C] FIG. 8C is a schematic diagram of an example of a weighting function used to generate the colored paths of FIG. 8B. [Figure 9A] FIG. 2 is a schematic plan view of an example of a 3D point cloud used to generate a colorized cost map. [Figure 9B] FIG. 9B is a schematic plan view of an example of a colorized cost map generated from the point cloud of FIG. 9A. [Figure 9C] FIG. 1 is a schematic plan view of an example of a path calculated using conventional cost calculations. [Figure 9D] FIG. 13 is a schematic plan view of an example of a path calculated using a map restrictor cost calculation; [Figure 9E] FIG. 13 is a schematic plan view of an example of a path calculated using a colorized cost map. [Figure 10A] FIG. 1 is a schematic perspective view of an example of a 3D point cloud used to generate a colorized cost map. [Figure 10B] FIG. 10B is a schematic plan view of an example of a colorized cost map generated from the point cloud of FIG. 10A. [Figure 10C] FIG. 1 is a schematic plan view of an example of a path calculated using conventional cost calculations. [Figure 10D] FIG. 13 is a schematic plan view of an example of a path calculated using a map restrictor cost calculation; [Figure 10E]FIG. 13 is a schematic plan view of an example of a path calculated using a colorized cost map. [Figure 11A] FIG. 1 is a schematic perspective view of an example of a 3D point cloud used to generate a colorized cost map. [Figure 11B] FIG. 11B is a schematic plan view of an example of a colorized cost map generated from the point cloud of FIG. 11A. [Figure 11C] FIG. 2 is a schematic plan view of an example of a first path calculated using conventional cost calculations. [Figure 11D] FIG. 13 is a schematic plan view of an example of a first path calculated using a colorized cost map. [Figure 11E] FIG. 13 is a schematic plan view of an example of a second path calculated using conventional cost calculations. [Figure 11F] FIG. 13 is a schematic plan view of an example of a second path calculated using a colorized cost map. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0039] An example of a method for generating a colorized cost map for use in planning a path through an environment will now be described with reference to FIG.

[0040] For purposes of illustration, it is assumed that the processes are at least partially performed using one or more electronic processing devices forming part of one or more processing systems, such as a computer system or the like, which may optionally be connected to one or more other devices via a network architecture or the like. For ease of illustration, the remainder of the description will refer to a processing device, although it will be understood that multiple processing devices may be used, with processing distributed among the devices as necessary, and that singular references encompass plural configurations and vice versa.

[0041] In this example, in step 100, the processing device obtains a map of the environment. The manner in which this is accomplished will vary depending on the preferred embodiment, and may include, for example, obtaining a pre-generated map from a database or other repository, and generating the map based on mapping data or a point cloud or the like. Such techniques are known in the art, and therefore will not be described in further detail. However, it should be noted that the map of the environment is typically a two-dimensional environment map, examples of which are well known in the art.

[0042] In step 110, the processing device acquires traversal data indicative of a capability to traverse a portion of the environment. The characteristics of the traversal data and the manner in which it is acquired will vary depending on the preferred implementation. For example, the traversal data could be acquired by having one or more vehicles traverse the environment while the vehicles are guided autonomously, semi-autonomously, or driven by an operator. In this example, the traversal data could be collected by on-board sensors, including without limitation GPS sensors, radar, LiDAR, or the like, and could represent the vehicle's trajectory through the environment.

[0043] Additionally and / or alternatively, the traversal data could be captured by static sensors provided in the environment, such as cameras or other imaging devices or the like positioned within the environment. However, this is not required and other approaches could be used, such as by having the user manually sketch possible vehicle trajectories or otherwise input details of parts of the environment that may or may not be traversed. For example, this could include having the operator use their own domain knowledge to trace paths that may be driven by the vehicle through the environment, and optionally, in addition, overlaying any applicable rules, such as road traffic rules or the like.

[0044] In one particular example, the traversal 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 traverses the environment to collect range data indicative of ranges to features within the environment. The range data can then be decomposed into a point cloud and trajectories through the environment by using a simultaneous localization and mapping (SLAM) algorithm that allows the range data to be used to generate both the map and the traversal data.

[0045] In step 120, the processing device uses the traversal data to calculate a traversal indicator that indicates the ability to traverse different parts of the environment. The traversal indicator is calculated in a color space such that the indicator is a color that represents the ability to traverse that part of the environment. The ability to traverse a part of the environment may be, for example, binary, indicating that the part of the environment may or may not be traversed, but is more typically graduated to reflect restrictions on traversal, for example to indicate directionality and the degree to which the directionality applies. For example, lanes on a motorway or highway may have a high degree of unidirectionality, while a one-lane road may be bidirectional. Finally, a car park or other similar area may have multidirectionality.

[0046] The portion for which the indicator is determined is typically a small region, such as a group of one or more pixels in the final cost map, and is typically based on the portion of the path traversed by the vehicle or the like, depending on the characteristics of the traversal data. Thus, performing this process for multiple portions of the environment that may be traversed allows the environment map to be colorized in step 130, thereby generating a colorized cost map indicative of the ability to traverse the environment.

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

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

[0049] Thus, the above-described scheme operates to utilize information regarding traversal of an environment to generate a colorized cost map to represent the ability to traverse the environment. In this regard, it should be understood that references to the ability to traverse an environment may encompass not only physical limitations on movement through the environment, but also any rules or the like applicable to traversal of the environment.

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

[0051] Some further features will now be described.

[0052] As mentioned above, in one example, the colored cost map is colored to indicate the direction of permitted movement within different parts of the environment, and specifically, different permitted directions of movement are indicated by different colors. This allows visual inspection to be used to quickly identify whether a proposed path satisfies a directional requirement. Specifically, the direction of a proposed path can be compared to the colors displayed on the colored cost map to determine whether the directional requirement is satisfied. While the following example will focus on directionality, it should be understood that a similar approach can also be used to display other constraints on traversal through an environment. For example, a slope could be colored to allow the use of visual inspection to identify whether a ramp can be traversed by a vehicle. In other examples, color information could be used to represent constraints on speed, velocity, acceleration, or other related information.

[0053] In one example, the color space is hue / Saturation A color space, specifically, hue / Saturation / brightness (HSV) color space or hue / Saturation / brightness The preferred color space is the High-Speed ​​Light (HSL) color space. From the following discussion, it will be appreciated that HSV is particularly useful because it allows for the conveyance of different ranges of information, but this is not essential 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 scaled such that directionality is indicated by the hue component and directional costs are Saturation It is colored as indicated by the components. Saturation While different orientations can be mapped to RGB colors, in situations where color directionality means that it must be strictly adhered to, the relative low Saturation This means that there is a relatively large degree of flexibility regarding orientation. As will become clear from the following discussion, areas of the map where no orientation restrictions exist will usually have a minimum of Saturation As a result, it will appear in white. High brightness without constraints Therefore, in the state with a white appearance, to indicate the obstacle cost, brightness Impassable obstacles that cannot be traversed while components can be used will have a black appearance.

[0055] In one example, the traversal data is indicative of one or more paths within the environment. In this respect, the term path should be understood to encompass a single traversal along a given route within the environment, such 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] Having identified the path from the traverse data, the processing unit is configured to use the traverse data to calculate a path vector at each of a plurality of points along the path, the path vector indicating a direction of movement at that point on the path, and which is generated in color space such that the path through the environment is effectively transformed into a sequence of color space path vectors distributed along the length of the path.

[0057] In one example, the processing device may be configured to determine a traversal direction at a point along the route from the traversal data, which may involve, for example, determining a vehicle trajectory from trajectory data indicating the trajectory of a vehicle moving along the route, and then determining the traversal direction using the trajectory, for example, by extrapolating the direction of movement from the trajectory or by deriving a pose of the vehicle from the trajectory data, depending on the characteristics of the data.

[0058] Having determined the direction of movement, this is then used to calculate a path vector by converting the traversal direction into color space, which is typically achieved using a mapping that relates different directions to different hue values, and an example of this will be described in more detail below.

[0059] Having calculated the path vectors at multiple points for each path, the processing unit can combine the path vectors from the multiple paths in individual portions of the environment, which are then used to generate a traversal indicator. Thus, in this example, path vectors from the multiple paths are combined where the paths overlap, thereby generating traversal indicators in different portions of the environment, which can then be collectively used to color the cost map.

[0060] It should be appreciated that combining path vectors from different paths allows for a relatively better understanding of directional constraints. For example, on a one-lane road, a single crossing of the road would result in a high degree of directionality being assigned to the road. However, if the road is a one-lane two-way road, i.e., is actually bidirectional, this would only be identified if multiple paths along the road corresponding to vehicles traveling in different directions were considered. Similarly, on a one-way road, considering multiple paths along the road would avoid vehicles traveling in the wrong direction unduly influencing the directional cost associated with that road.

[0061] The path vectors are typically combined in different color spaces, with the crossing indicator being generated by transforming the result of the combination step into the color space, so for example, the path vectors are generated in HSV space which may be combined in RGB space, allowing linear combinations of the path vectors to be used to generate meaningful colors, with the resulting combined colors being transformed back into HSV space.

[0062] The path vectors may be combined in any suitable manner, but in one example this is done by averaging the path vectors, and specifically by utilizing a weighted average. In this regard, weighting can be used to take into account the fact that directionality is not usually confined to a discrete point, but rather applies more generally to an area around a point. However, the further away from the point, the less certainty associated with the directionality is, and this can be reflected by using weighting. This effectively combines several path vectors surrounding each point on the path, with path vectors further away from the point having relatively greater uncertainty in the directionality, and therefore reduced weighting.

[0063] In one example, this is accomplished by spreading out the direction of movement associated with the vehicle around a point on the path, such that an assumption is made that points adjacent to the point currently under consideration will also have similar directional requirements. It will be appreciated that any imprecision in this approach is typically compensated for by averaging path vectors from multiple different paths within any given portion of the environment. This also allows the directional assumptions to be propagated 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, as will be described in more detail below.

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

[0065] As mentioned above, the system can be configured to allow path planning to be carried out, and an example of this process will now be described with reference to FIG.

[0066] In this example, a start and a finish are determined to represent the beginning and end of the route in step 300. The start and finish may be determined in any particular manner, but in one example this is accomplished by having them manually defined, although alternatively they could be obtained from pre-stored data or the like.

[0067] At step 310, the processing unit may generate at least one candidate route. The route may be generated using various route planning techniques, such as a map restrictor technique, and may optionally be calculated using a colorized cost map by using color as a cost function, as described in more detail below. The candidate routes may then be displayed at step 320, optionally to allow a user to perform a visual inspection at step 330, thereby evaluating one or more candidate routes.

[0068] This approach therefore allows the colorized cost maps to be used in an autonomous and / or semi-autonomous manner, and in one particular example allows visual inspection to be used to quickly assess the variability of an autonomously derived path.

[0069] In one example, the processing device is configured to generate a path using a cost map. Generating a path using the cost map can generate a relatively optimal path (shorter / faster / avoids dangerous obstacles). The difference between the standard path and the optimal path generated by the cost map is greater when the traversed environment contains multiple obstacles that are dangerous or difficult to overcome (in which case the optimal path may be counter-intuitive to the user when initially viewing the map of the environment).

[0070] In one example, the processing device is configured to navigate and / or control the vehicle through the environment according to a path and / or cost map generated using the cost map, allowing the vehicle to drive autonomously through the environment while taking the cost map into account, for example, by following a path planned using the cost map or by dynamically controlling the vehicle using the cost map to avoid obstacles, ensure that one-way streets are navigated in the correct direction, or the like.

[0071] However, it should be understood that this is not required and that the cost map may be used to assist an individual driving a vehicle through an environment. In this example, the cost map would be presented on a screen in the vehicle, which would show the user where the vehicle may or may not traverse. A conventional map of the environment would not normally show this information, especially if there are variations in altitude or the presence of obstacles that may prevent the vehicle from traversing. By presenting navigation instructions to the user, the user only needs to focus on the direction of the vehicle, without having to interpret the map and invent a way to traverse the environment.

[0072] In one example, the processes are performed by one or more processing systems, and an example of a suitable processing system 420 is shown in FIG.

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

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

[0075] It should therefore be understood that processing system 420 may be formed from any suitable processing system, such as a suitably programmed client device, a PC, a web server, a network server, or the like. In one particular example, processing system 420 is a standard processing system, such as a processing system based on the Intel architecture, which executes software applications 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, a microchip processor, a logic gate configuration, firmware optionally associated with implementation logic such as an FPGA (Field Programmable Gate Array), or any other electronic device, system, or configuration.

[0076] However, it should be understood that the above configuration assumed for purposes of the examples that follow is not required, and that numerous other configurations may be used. For example, the process may be implemented using a distributed configuration, such as a client-server configuration, a cloud-based or edge computing configuration, or the like, depending on the particular implementation.

[0077] A specific example of the above process of generating a colorized cost map for encoding directionality within the cost map and then subsequently using this for path planning will be described in more detail below.

[0078] A specific example of a processor for generating a cost map will now be described with reference to Figures 5A and 5B.

[0079] In this example, in step 500, processing system 420 retrieves traverse data from database 426. The traverse data represents data collected from vehicles moving along routes through an environment and could be based on any location data such as GPS coordinates, or mapping data such as LiDAR data, or the like. The traverse data typically includes several different routes, which could be a route 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 next path for analysis and then segments the path to select points on the path in step 510. The points are typically spaced a predetermined distance apart, which is used in this case to avoid analyzing the entire length of the path, thereby reducing computational requirements.

[0081] The processing system 420 determines the direction of travel at each point in step 515. The direction of travel could be the orientation or pose of the vehicle (or a sensor mounted thereon) at each point, if known, and / or could be derived based on the trajectory traversed by the vehicle. For example, if the traverse data includes only location information, such as a sequence of GPS coordinates or the like, it may be necessary to interpolate these to derive the travel path and therefore estimate the vehicle's orientation.

[0082] In step 520, the processing system 420 operates to spread the direction of movement to effectively "spread" the direction of movement to an area surrounding the point at which the direction of movement is being determined. Because confidence in the correct direction of movement decreases with distance from the point, the directions are spread according to merge weights in step 525, as described in more detail below.

[0083] This process is repeated for all points along the path, and when a path is completed, it is determined whether all paths are complete in step 530. If not, the process returns to step 505 to analyze the next path.

[0084] Once all paths have been completed, the processing system operates to average, in step 535, in RGB color space, the path vectors of a portion, such as a group of pixels in the environment map. The averaging is performed according to merge weights such that vectors that are farther from the point of actual measurement are given relatively less weight throughout the averaging process. The averaging process is used to generate an average vector in step 540, which is then converted to HSV color space in step 545 to form a traversal indicator for each portion of the environment. Once this is performed, the traversal indicator is added to the cost map in step 550 to allow the cost map to be colorized.

[0085] Specific details of the preferred implementation will be described in more detail below.

[0086] As mentioned above, the global planner operates on a known map M. In navigating through map M, the vehicle navigates only through feasible areas due to physical, legal, or arbitrary constraints. Based on this inherent characteristic of how / where the vehicle must travel, the method attempts to extract information from the vehicle trajectory (by assuming the vehicle has a localization system that provides such a trajectory) to incorporate preferred directionality into the map. The main challenge is how to effectively represent directionality in a way that can be mathematically interpreted by a path planning algorithm, but also 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 a sensor as it moves through the environment. Thus, any map M will have a trajectory T associated with it, where the trajectory T is the path followed by the sensor in 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 usually performed by sensors mounted on the vehicle itself (unlike maps generated through, e.g., handheld or aerial platforms). Thus, in the mapping process, the vehicle (and therefore the sensor) navigates only through areas that are feasible due to physical, legal, or any constraints. Based on this inherent property of the way the map is generated, the method can extract information from the trajectory to incorporate preferred orientations in the map used for global planning.

[0088] Directionality is then represented in a manner that can not only be mathematically interpreted by a path planning algorithm, but also intuitively understood by a human observer of the map. Because 2D path planning algorithms for ground vehicles often analyze cost maps represented in image data structures, the present scheme represents directionality as different colors in the map.

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

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

[0091] 6C shows an example of a resulting cost map generated by analyzing multiple paths, which in this case shows paths 601 colored according to the direction of vehicle movement along the path. Thus, in this example, hue (H) is used to represent the direction the vehicle can, and therefore should, travel when traversing the path.

[0092] While only hue may be used, more commonly Saturation (S) and brightness The component (V) is also used, in which case: Saturation The components define the importance of directionality on a particular route (e.g., there are areas where a vehicle may proceed in both directions without any consequences, and other areas where proceeding in a given manner is absolutely prohibited). In Fig. 6C, low directionality importance is shown in the "white" route segments 601.1. In these segments, vehicles traverse in multiple directions, thus indicating the "bidirectional" nature of the route, and as a result, Saturation Finally, brightness The component represents the overall cost in the map, which includes obstacles (i.e., shown in black). Low brightness represents an obstacle).

[0093] In one example, a modified A* algorithm is used to generate the cost map such that the mathematical formulation incorporates HSV costs in the optimization rather than just free vs. non-free space costs. While this method adds minimal computation to the map generation process, it potentially increases the computational complexity of 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, although it should be understood that this is not required and that it may be applied relatively broadly to any data set that may be used to derive a viable vehicle trajectory through an environment.

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

[0096] However, this type of restriction is directional and simple. This simplicity is illustrated in the experiments in Figures 9 and 10, described in more detail below, which show how a cost map can be modified to give preference to previously traversed routes. Throughout the following description, this route biasing strategy is referred to as the "map restrictor" approach.

[0097] A standard cost map is a scalar field that represents the cost of passing through each point in the field. Typically, the field is discretized as a two- or three-dimensional grid of values. These values ​​determine the cost of traversing a grid cell, and the path planner then aims to minimize the integrated cost along the path length. However, there can be ambiguities in the way these costs are represented.

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

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

[0100] In the present scheme, a direction "utility value" is calculated from the dot product (.) of the candidate vehicle direction vector with the color vector for each pixel, resulting in a signed value in the range [-1,1]. However, since the utility value is preferably in 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 a color map may be derived from multiple driving paths through the environment, and linearity ensures that such averaging of color vectors corresponds to the average of the costs of these vectors.

[0101] The present scheme uses the HSV color space depicted in Figures 7A and 7B, which represent the traditional hue and Saturation , and brightness FIG. 1 shows representations and representations relating color spaces to variables used in the description of this work. In this example, hue H represents the intended driving direction, Saturation S defines the directional weights, and brightness V represents the overall cost of the cell, C. For a given candidate direction, d, the cost is:

number

[0102] This is a conic subset of vector space, and examples of this are hue, Saturation , and brightness This is shown in FIG. 7A which shows a conventional representation having the color space ##EQU1## and FIG. 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 direction of movement) and white represents the lowest cost. Light colors represent the lowest cost when moving in the direction that corresponds to the color's hue. This option fits the driving task because the highest cost areas (obstacles) are costly regardless of approach direction, while the lowest cost areas (roads, open fields, and other vehicle operating areas) are those where directionality may be appropriate. In addition to loosely constraining direction by the hue vector, brightness By using the component V, the driving direction can be hard constrained. As the environment is mapped, obstacles are marked as black (full cost) and all remaining areas are marked as gray (medium cost). The driven trajectory is colored according to the driving direction, and the color is interpolated to a predefined gray color depending on the distance from the trajectory path. This generates a set of soft-constrained routes to constrain the driving direction.

[0104] In omnidirectional areas such as a campus or car park, the average of multiple driving passes on the same route in different directions produces an averaged white color, as shown in 601.1 in Figure 6C. This is the default for non-obstacle areas. brightness The gray is lighter than the gray used as a directional color, which means that the vehicle will still prefer to drive on the area covered by the vehicle crossing without any additional directional cost. However, this does not necessarily prohibit the vehicle from navigating into the gray area, since the system can be parameterized to give a relatively small preference only to gray in the case where the goal is located within one of the regions, but not to avoid it completely.

[0105] Being a cone color space, HSV allows the expression of directionality (hue), direction certainty (color shading), and color shading in a format that is interpretable and editable by a human operator. Saturation ), and obstacles ( brightness ) brightness The addition of obstacle layers in the form of components allows the planner to be biased towards pre-driven paths rather than limiting it to these paths. The use of direction certainty, rather than just direction, addresses the ambiguity of intersecting paths and bidirectional movement.

[0106] In generating the cost map, as described above, the vehicle path is segmented into individual points, where the direction of movement at each point is used to generate vectors. An example of this is shown in Figure 8A, which shows the vehicle heading 841 indicating the direction of movement in each of a number of frames 842 representing individual points on the path.

[0107] To provide the above-mentioned smooth transition from a color-saturated path to a predefined background color and reduced saturation in a bidirectional area, -Spread of the trajectory, and Color averaging, Two major processing steps must be completed:

[0108] To generate smooth transitions, a radius of linearly decreasing merge weights α with a maximum distance r is added to each pose in the trajectory T. To generate smooth transitions, a linearly decreasing "merge weight" α with a range of 1 to 0 generally reduces saturation of the path. α is parameterized according to the maximum distance r from the center of the trajectory T, as shown in FIG. 8C. In this example, a linear decrease in weight with distance is used, but this is not required, and other weightings such as a bell curve or other distributions could be used. The direction of movement θ is extracted from the trajectory T, and a color representation vector c is calculated for each point within a distance r from the center of the trajectory T.n In this case:

number

number

number

number

number

[0109] Since there can be multiple vectors / pixels, an index n is assigned to each to discriminate between them. A weighted sum of the vectors generated from T and the background color is computed to effectively average the list of colors generated and generate colors that are not saturated in bidirectional areas. By averaging the x and y components of the color rather than the direction, the saturation of the final color can be reduced when the summed components work in opposite directions. In addition to this, averaging the z component can achieve a smooth transition to the background color.

[0110] c Background is defined as a vector containing the desired background color, so for example, if the desired background color is gray, one could use the vector (0,0,0.7). In this example:

number

number

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

number

number

[0112] Then, this vector c Average has been transformed back into the HSV color space according to the following formula:

number

number

number

[0113] The resulting HSV values ​​are incorporated into a cost map that is used in path planning.

[0114] Thus, a direction (pose) is extracted from the trajectory before it is unfolded as described in Figure 8C. For each point in this unfolded direction (pose), a color vector is calculated and then averaged using the formula above. The resulting values ​​for each pixel are converted to HSV before being written out to be used as a cost map in the plan.

[0115] As described below, several experiments were performed to test the cost map and path planning functions.

[0116] Specifically, we present several scenarios that compare the above approach with standard cost calculation approaches, thereby illustrating the applicability of the method.

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

[0118] Three main scenarios were chosen to evaluate and demonstrate the difference in behavior between our scheme, the basic map restrictor scheme, and traditional cost calculation. These scenarios include various normal road networks, industrial areas, and off-road environments. Traditional cost calculation scenarios imply a free vs. non-free binary cost map representation.

[0119] Scenario #1 depicted in Figures 9A-9E includes a roadway 901 with lanes used to generate the cost map and the cost map shown in Figure 9B with color mapping 910. The goal point G is located approximately 50 meters away from the start point S. However, the shortest path to the goal is proceeding in the opposite direction of travel according to the site rules. When direction is not important, a short direct path is desirable, as illustrated by the paths generated by the traditional cost calculation and map restrictor methods, as shown in Figures 9C and 9D, respectively. When direction is important, the path taken by the present method is significantly longer since it performs a full loop, but it adheres to the encoded directionality rules, so that it always stays on the left hand side of the road, as shown in Figure 9E.

[0120] Scenario #2, shown in Figures 10A-10E, again includes the cost map shown in Figure 10B, which includes roads 1001 and color mapping 1010. The goal point G is located approximately 50 meters away from the start point S. This example shows similar results with both the routes taken by the map restrictor and the traditional cost calculation method shown in Figures 10C and 10D violating traffic rules. In contrast, the route taken by the method shown in Figure 10E is much longer, but adheres to these rules. This scenario also highlights the difference between the map restrictor and the traditional cost calculation, where the route taken by the map restrictor is much smoother and more closely follows the pre-driven route, a known plausible route.

[0121] Scenario #3, shown in Figures 11A-11F, consists of a highly sloping off-road area. The cost map, shown in Figure 11B, includes the path 1101 traversed and the color mapping 1110 used to generate the cost map. When considering a method for traversing and climbing a slope, the desired path is often indirect and follows a relatively gentle uphill slope. Scenario #3 shows both the map restrictor, shown in Figure 11D, and the path taken by the present method, both of which follow a safe route encoded by past experience, thus following a gentle gradient. In contrast, the route taken by the conventional cost calculation, shown in Figure 11C, leads the vehicle into the steepest section of the slope, thus putting the vehicle at risk of rollover. A secondary feature of off-road environments is that the path is often one-way. This property is again represented by both the map restrictor and the present scheme, which allows the vehicle to traverse down the slope and along the flat section of the path, as shown in Figure 11F, rather than putting the vehicle at risk by moving along the slope, as in the conventional cost calculation case of Figure 11E. Note that in all figures showing the experimental trajectories, inverted colors are displayed for visualization purposes.

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

[0123] [Table 1]

[0124] [Table 2]

[0125] In Table 1, it can be seen that the additional time used in including a colorized path depends on the size of the map and the number of trajectory points. This time can be seen to vary from about 1.8 to 3.5 times the time required for a non-colorized map.

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

[0127] Thus, the above-described scheme provides an efficient way to embed directional characteristics in a cost map. The scheme is applicable to map-based navigation, and is therefore particularly useful in areas requiring autonomous navigation within a known map. In one example, the scheme uses past travel directions to relate the directional characteristics of the vehicle's travel to a directionality in HSV color space to colorize the cost map, which, once colorized, is embedded with the directional preference.

[0128] In one particular example, the method uses hue, hue, and scalar cost to encode the direction, orientation, and scalar cost, respectively, into a cost map image. Saturation , and brightness By using the components, the vehicle's orientation angle and hue - Saturation - brightnessIt generates similarities between hue angles in color space. The cost calculation function can be used by the A* algorithm to incorporate this information into the path planning algorithm to allow path 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 the scheme allows proper path planning while standard binary cost maps fail.

[0129] While the above examples focus on directionality, the method can extrapolate to include any other information from the trajectory in the map, such as speed, acceleration, or any other 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" are to be understood to mean the inclusion of the stated integer or group of integers or steps but not the exclusion of any other integer or group of integers. As used herein, the term "about" means ±20% unless otherwise stated.

[0131] Those skilled in the art will appreciate that numerous modifications and variations will be apparent to those skilled in the art, and all such modifications and variations which become apparent to those skilled in the art are intended to be within the spirit and scope of the invention as broadly described above.

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 ability 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 ability 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 trajectories 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 ability 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 ability 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