Method, control arrangement and vehicle for providing a path using rapidly exploring random tree algorithms

Abstract

A method and control arrangement (10) for providing a path of an autonomous vehicle (1) in a defined environment, the autonomous vehicle (1) having certain dynamics and kinematics. The method comprises obtaining (S1) a starting configuration and a goal configuration for the autonomous vehicle and searching (S7) the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The searching comprises generating random configurations in the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle, generating one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler, and using the one or more new goal configuration for finding the feasible path with the RRT algorithm. The method further comprises providing (S8) a path for the autonomous vehicle using a result of the searching (S7). The disclosure also relates to a vehicle (1) comprising the control arrangement (10).

Description

[0001]Method, control arrangement and vehicle for providing a path using Rapidly Exploring Random Tree algorithms Technical Field The present disclosure relates to path planning for autonomous vehicles, and in particular to methods, control arrangements and a vehicle for providing paths for an autonomous vehicle using Rapidly Exploring Random Tree, RRT, algorithms. Background For autonomous vehicles, motion planning is a crucial aspect of their operation. Motion planning includes finding a feasible path to reach a goal and developing a trajectory that the vehicle can follow along the path. There exists a plurality of different approaches to path planning and trajectory development. One approach is using Rapidly exploring Random Tree (RRT), algorithms. The basic idea behind such algorithms is to generate random sampling configurations in a configuration space and connect all sample configurations in a tree-like structure with feasible paths between each pair of sample configurations. A path can then be extracted between a desired starting configuration and goal configuration from the tree-like structure where the root of the tree-like structure is the starting configuration, and the top of the tree is the goal configuration. RRT builds the tree incrementally, expanding it towards unexplored regions of the configuration space. For non-holonomic systems like a vehicle, such incremental growth can naturally capture feasible trajectories that follow non-holonomic constraints. Many improvements have been made to the standard RRT algorithm. For example, RRT*, Goal Oriented RRT (GO-RRT), RRT Connect, RRTx and Rapidly exploring Random Vines (RRV). Currently, all improvements of the standard RRT algorithm are made to increase computational efficiency or the quality of the solution between two configurations. Summary Non-holonomic systems must obey their constraints. For example, a vehicle has a constraint on its lateral motion as it cannot move sideways without additional movement in another direction. This can result in impossible missions for the vehicle, e.g., a short mission in a curve that is tighter than the maximum allowed curvature of the vehicle. It is an objective of the present disclosure to provide an improved path planning method compared to prior known methods. It is a further objective to provide an improved path planning method that is based on the RRT algorithm technique. These objectives and others are at least partly achieved by the method, control arrangement, and vehicle according to the independent claims, and by the embodiments according to the dependent claims. According to a first aspect, the disclosure relates to a method for providing a path of an autonomous vehicle in a defined environment. The autonomous vehicle has certain dynamics and kinematics. The method comprises obtaining a starting configuration and a goal configuration for the autonomous vehicle and searching the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The searching comprises generating random configurations in the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle. The searching further comprises generating one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler and using the one or more new goal configuration for finding the feasible path with the RRT algorithm. The method further comprises providing a path for the autonomous vehicle using a result of the searching. The method removes unnecessary deadlocks in autonomous driving caused by unfeasible missions for the autonomous vehicle, by changing the mission itself. This is based on the insight that it is often not needed to reach the mission’s goal configuration with total accuracy. Instead, if a feasible path to the mission’s goal configuration cannot be found, a new goal configuration in the vicinity of the mission’s original goal configuration is randomly sampled that is used instead. Thereby the autonomous vehicle can still find a path sufficiently close to the original goal to complete the mission and a deadlock is avoided. According to some embodiments, the searching comprises using the one or more new goal configuration based on a desired design of the RRT-algorithm. The RRT-algorithm may thus be designed differently to take advantage of new sampled goal configurations. According to some embodiments, the RRT-algorithm is goal-biased and searches the defined space in an iterative manner, wherein the searching comprises exchanging the random configuration for the goal configuration in a percentage of the iterations. The RRT- algorithm will then be biased towards the goal and thus weighted to find a path more efficiently towards the goal configuration. According to some embodiments, the searching comprises exchanging the goal configuration for one of the one or more new goal configurations according to a distribution rule. Thereby a new goal configuration can be tried out by the method. According to some embodiments, the distribution rule includes one or more of: i) increasing a number of replacements of the goal configuration with the one or more new goal configurations the closer the tree comes to the goal configuration; ii) replacing the goal configuration with one of the one or more new goal configurations upon failure to find a feasible path within expiration of an iteration limit or time limit; or iii) replacing the goal configuration with the one or more new goal configurations. Thereby the goal configuration can be altered based on different design alternatives. According to some embodiments, the defined environment includes road network and geospatial data. The RRT-algorithm will thus operate in an environment that is suited for autonomous driving. According to some embodiments, the method comprises one or more of: i) obtaining dynamics and kinematics of the autonomous vehicle, ii) obtaining a defined environment of the autonomous vehicle, and iii) obtaining a step size of the RRT-algorithm wherein the searching comprises using the step size for finding the feasible path with the RRT algorithm; iv) obtaining obstacle information and wherein the searching comprises using the obstacle information for finding the feasible path with the RRT algorithm, and v) obtaining real-time sensor data wherein the searching comprises using the real-time sensor data for finding the feasible path from the starting configuration to the goal configuration. According to some embodiments, the providing a path for the autonomous vehicle using a result of the searching comprises tracing back from the goal configuration to the starting configuration along intermediate tree connections defining the determined path. According to a second aspect, the disclosure relates to a computer program comprising instructions which, when the computer program is executed by a computer, cause the computer to carry out the method according to the first aspect. The same advantages as have been described above with regards to the first aspect are thereby achieved. According to a third aspect, the disclosure relates to a computer-readable medium comprising instructions which, when executed by a computer, cause the computer to carry out the method according to the first aspect. The same advantages as have been described above with regards to the first aspect are thereby achieved. According to a fourth aspect, the disclosure relates to a control arrangement for providing a path of an autonomous vehicle in a defined environment. The autonomous vehicle has certain dynamics and kinematics. The control arrangement is configured to obtain a starting configuration and a goal configuration for the autonomous vehicle and search the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The search comprises to generate random configurations within the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle and to determine a path for the autonomous vehicle using a result of the search. The control arrangement is further configured to search the defined environment comprising to generate one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler and use the one or more new goal configurations to find the feasible path with the RRT algorithm. The same advantages as have been described above with regards to the first aspect are thereby achieved. According to a fifth aspect, the disclosure relates to a vehicle comprising the control arrangement according to the fourth aspect. The same advantages as have been described above with regards to the first aspect are thereby achieved. The embodiments disclosed herein are illustrated by way of example, and not by way of limitation, in the figures of the accompanying drawings. Like reference numerals refer to corresponding parts throughout the drawings, in which Figs.1a-1d illustrate an example search of an RRT algorithm. Fig.2 illustrates an exemplary autonomous vehicle. Fig.3 is a flowchart of a method according to some embodiments of the disclosure. Fig.4 illustrates a defined environment and a goal configuration of an autonomous vehicle. Fig.5 illustrates an optimal trajectory, a system curvature constraint and a plurality of goal configurations. Fig.6 illustrates a control arrangement and related devices according to some embodiments of the disclosure. Detailed Rapidly Exploring Random Tree (RRT) is an algorithm that can be used in path planning to find a feasible path from a starting configuration to a goal configuration. In general, the RRT algorithm gradually builds the path by building a tree structure, herein referred to as a “tree”, from the starting configuration to the goal configuration. To build the tree, the algorithm searches a configuration space ^^ by randomly sampling new configurations and attempting to extend the tree with these new configurations. A configuration in the tree may also be referred to as a node. The starting configuration may then be referred to as a root node and the goal configuration as a goal node. The algorithm continues until a termination condition is met, for example that a maximum number of vertices has been reached, that a time limit or iteration limit has expired or that a goal configuration has been reached. For better understanding of RRT, a prior known basic RRT algorithm is explained below as “Algorithm 1”. This algorithm is retrieved from the “The RRT Page” on “lavalle.pl / rrt / about.html” on September 18, 2023. A visualization of the development of a tree with such a basic RRT algorithm is illustrated in Figs.1a to 1d. After a termination condition is met, here that a maximum number of iterations has been reached, the planned path may be generated by the GET_PATH function, as shown in “Algorithm 2” below. Algorithm 1: Build_RRT (^^^^^௧,^^,^^^^^ 1. ^^_init (^^^^^௧) 2. for ^^ ൌ 1 to ^^3. ^^^^^ௗ←RAND_CONF ( ) 4. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 5. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 6. ^^.add_vertex(^^^^௪^; 7. ^^.add_edge(^^^^^^, ^^^^௪^;8. Return ^^ The “Algorithm 1” is named “Build_RRT” and builds a tree “^^” from a starting configuration ^^^^^௧as a root node with ^^ vertices and a step length of ^^^^. Number 1 to 8 outlines different steps of the algorithm with pseudocode as used on the “The RRT Page” and also widely recognized from other sources of literature with more or less the same notation. In step 1 the tree ^^ is initialized with the starting configuration ^^^^^௧as the root node. Fig.1a illustrates such a starting configuration ^^^^^௧as a starting point in aconfiguration space, here a square. Step 2 outlines that for ^^ ൌ 1 to ^^ times, the steps 3 to7 are performed. ^^ is thus the maximal number of iterations. In step 3, a RAND_CONF function generates a random sampling configuration ^^^^^ௗin the configuration space of the tree ^^. In some embodiments, the RRT is goal biased. This means that in a percentage “x” of all the random sample configurations ^^^^^ௗ, ^^^^^ௗbecomes the goal configuration ^^^^^^instead of a new random sample configuration. This percentage “x” is in the standard case around 10%. This variant, i.e., goal-bias, of step 3 in “Algorithm 1” is here exemplified as “Algorithm 1a” named “Goal biased RRT-steps” below, described in steps 1 to 4 with pseudocode: Algorithm 1a. Goal biased RRT-steps 1. if ^^ > ^^^^^^then 2. ^^^^^ௗ←RAND_CONF ( ) 3. else 4. ^^^^^ௗ= ^^^^^^; where ^^^^^^is the probability threshold (e.g., x=10 % becomes 0.1 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^. If ^^ > ^^^^^^, a random configuration ^^^^^ௗis obtained by uniformly sampling in the configuration space using RAND_CONF, otherwise ^^^^^ௗis set to ^^^^^^. A RAND_CONF function may herein be referred to as a random sampler. The random sampler is for example a Gaussian sampler, which is a commonly known sampling technique. A Gaussian sampler generates samples from a Gaussian distribution. Returning to Algorithm 1, in step 4, the nearest vertex in the tree to the random sampled configuration ^^^^^ௗis found. For example, a NEAREST_VERTEX function selects an extension node ^^^^^^amongst the existing tree nodes that is the nearest node to ^^^^^ௗ, and a line is drawn to connect ^^^^^ௗand ^^^^^^. In step 5, the tree is extended from the nearest node ^^^^^^already in the tree towards the sampled configuration ^^^^^ௗ. For example, a NEW_CONF function generates a new configuration ^^^^௪in the direction of the line using the step size ^^^^ while taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle. A collision check may be performed taking into account obstacles in the environment. If there is no collision between any obstacle and a line between ^^^^^^and ^^^^௪, then ^^^^௪is added to the tree as a new node in the tree, also referred to as a leaf node. The new configuration ^^^^௪is added to the tree using the ^^.add_vertex function in step 6. In a step 7, a new edge, i.e., a line, from ^^^^^^to ^^^^௪is added to the tree with the function ^^.add_edge. Figs.1b to 1d illustrate the progress of building a tree in the configuration space starting from the root node. Once the maximum number of iterations has been exhausted, a path from the start configuration to the goal configuration can be extracted by tracing back from the goal configuration to the root configuration along the tree connections. An example of such path finding is outlined in “Algorithm 2” below with steps 1 to 11 in pseudocode with the function GET_PATH (T). Algorithm 2: GET_PATH (T^ 1. Var path_set; 2. path_set.Add_Node (T.noden); 3. while True 4. i ← Parent_Node_Indexn; 5. path_set.Add_Node (T.nodei) 6. if i=0 7. break; 8. endif; 9. n ← i ; 10. endwhile 11. path ← path_set The function GET_PATH returns a path by iterating back from the goal node to the root node via the parents of the nodes. “i” denotes the index of the parent, and “n” denotes the child of the parent. Hence, each node in the tree is a child to a parent and vice versa, except the root node that does not have a parent and therefore its index “i” is zero. Hence, the function path_set.Add_Node adds the item numbers of the nodes to the path_set until the index becomes zero. The path_set then becomes the path in terms of item numbers of the nodes of the path. In the following the invention will be described in more detail. The invention is a development of the RRT algorithm, for example the RRT algorithm as previously explained or variants thereof, that is useful when a feasible path to the goal configuration cannot be found, and its use in autonomous vehicles. The invention is based on the insight that it is often not needed to reach the mission’s original goal configuration with total accuracy. Instead, one or more new goal configurations in the vicinity of the original goal configuration may be tested instead, which will generally not compromise the fulfillment of the mission as the new goal configurations are only slightly changed compared to the original goal configuration. For the purpose of explaining the invention, an autonomous vehicle 1 is presented in Fig.2, hereinafter referred to as “vehicle 1”, on a ground 4. An autonomous vehicle may be defined as a vehicle that can travel without human input. Vehicle 1 is for example a car, a truck, a bus or other kind of vehicle. In particular, vehicle 1 is a land / road vehicle. Vehicle 1 comprises wheels 2, 3, propulsion means (not shown), and other equipment known to the skilled person. For driving autonomously, vehicle 1 comprises sensors to sense its surroundings. In Fig. 2, reference number 2 generally denotes one or more sensors 2 positioned on vehicle 1 that can be used for autonomous driving, such as cameras, radars, lidars, and others, to provide a rich view of the surroundings. The one or more sensors 2 may be located at various locations on the vehicle 1. Vehicle 1 also comprises a control arrangement 10. The control arrangement 10 comprises control circuitry. Such control circuitry may comprise one or more electrical control units, ECUs, responsible for controlling one or more electrical systems or subsystems. The control arrangement 10 is herein generally explained as comprising processor 10a and memory 10b (Fig. 10). The control arrangement 10 includes, for example, a path planning module and a navigation module. The path planning module is responsible for generating a feasible path for the vehicle 1 based on a starting configuration and a goal configuration. The navigation module is responsible for operating the vehicle 1 according to the feasible path generated by the path planning module. The memory 10b stores at least one model of the vehicle 1 representing kinematics and dynamics of the vehicle 1. For example, the at least one model may include a kinematic model and / or a dynamic model of the vehicle 1. In one example embodiment, the vehicle 1 moves in a configuration space ^^ where theconfiguration is given by ^^^,^^, ^^^ where ^^ and ^^ represent the coordinates of the vehicle1, typically a characteristic point located midway of the rear wheels of the vehicle 1. ^^ represents the orientation of the vehicle 1. A kinematic model may be based on a kinematic based differential system as known to the skilled person, for example: where ^^ is the velocity, ^^ is the steering angle and ^^ is the distance between the rear axle and the front axle of the vehicle 1. In a dynamic model, the vehicle 1 may be represented with more degrees of freedom and therefor better characterize the movements of thevehicle 1. For example, the state of the vehicle 1 may be defined by ^^ ൌ ^^^ ்^, ^^^, ^^, ^^௬, ^^^ ,where ^^^and ^^^represents the coordinates of gravity of the vehicle 1, ^^ its orientation, ^^௬its lateral speed, and ^^ is the yaw rate. Based thereon, a dynamic model may be represented as: where ^^௫is the longitudinal speed and ^^௬is the lateral speed of the vehicle 1, and ^^ is the radius of the tires 3 of the vehicle 1. The configuration space ^^ of the vehicle 1 may be represented with an environment model, for example a two-dimensional world map where obstacles are represented as polygon lines or similar. A set of ^^ obstacles may be represented by ^^^^^ൌ ^^^^^^^ , ^^ ൌ 1 …^^^ , where the part of ^^ which is free of any obstacles is noted ^^^^^^ ൌത^ത^ത^ത^തത^. The configuration space ^^ may also be located in memory 10b.In the following a proposed method for providing a path of an autonomous vehicle 1 in a defined environment will be explained with reference to the flow chart in Fig.3 and the examples in Figs.4 and 5. The defined environment is for example the configuration space ^^ as has been previously described. The defined environment may for example include road network and geospatial data. The method is for example implemented by the path planning module of the control arrangement 10. The autonomous vehicle 1 has certain dynamics and kinematics. Such dynamics and kinematics may be represented by a kinematic model and / or a dynamic model of vehicle 1 as previously explained. To determine the path, a starting configuration and a goal configuration for the vehicle first have to be obtained. Thus, the method comprises obtaining S1 a starting configuration and a goal configuration for the autonomous vehicle. A configuration of the vehicle 1 may include its position in coordinates ^^ and ^^, and one or more of orientation ^^, velocity ^^, and steering angle ^^. These parameters may be available in vehicle 1, for example retrieved from different systems and / or sensors in vehicle 1 or saved in memory 10b. The parameters may be available via a communication network in the vehicle 1, for example a controller area network, CAN. For example, the position and orientation may be obtained from a global positioning system, GPS, (not shown) the velocity from a velocity sensor (not shown), and the steering angle from a steering angle sensor (not shown). The starting configuration is for example the present state of ^^, ^^, and optionally one or more of ^^, ^^ and ^^. The goal configuration is a desired state of ^^, ^^, and optionally one or more of ^^, ^^ and ^^. The method makes use of RRT techniques for finding a feasible path from the starting configuration to the goal configuration. This includes searching S7 the defined environment using an RRT algorithm. For that purpose, the method may include one or more of the following steps, before or during the searching is performed: i) obtaining S2 dynamics and kinematics of the autonomous vehicle 1. This is for example accomplished by retrieving such dynamics and kinematics from memory 10b in the control arrangement 10. ii) obtaining S3 a defined environment of the autonomous vehicle 1. This is for example accomplished by retrieving a previously defined configuration space ^^ from the memory 10b in the control arrangement 10. This may also be accomplished by retrieving a configuration space ^^ via an external source, for example maps from an external source communicated to the vehicle 1 via wireless communication. iii) obtaining S4 a step size of the RRT-algorithm and wherein the searching S7 comprises using the step size for finding the feasible path with the RRT algorithm. The step size is denoted ^^^^ in the described algorithms and may or may not be needed, depending on the design of the RRT-algorithm. iv) obtaining S5 obstacle information and wherein the searching S7 comprises using the obstacle information for finding the feasible path with the RRT algorithm. The method may continuously update the path based on new obstacle information by performing new searches with the RRT-algorithm where the configuration space ^^ has been updated with such new obstacle information. v) obtaining S6 real-time sensor data and wherein the searching S7 comprises using the real-time sensor data for finding the feasible path from the starting configuration to the goal configuration. The real-time sensor data may for example be retrieved from sensors 5 on vehicle 1 as previously described. This data may be analyzed for obstacles and information of such obstacles used by updating the configuration space ^^ and finding a new path with the updated configuration space ^^. The method further comprises searching S7 the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The searching comprises generating random configurations in the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle. Such random configurations are for example generated by a random sampler as previously explained. The searching S7 further comprises generating one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler and using the one or more new goal configuration for finding the feasible path with the RRT algorithm. The goal random sampler is a sampler separate from the random sampler. The goal random sampler is designed to generate new goal configurations in the vicinity of the original goal configuration. The goal random sampler is for example a Gaussian sampler. The Gaussian sampler may have some allowed deviation in position, both laterally and longitudinally, but very little allowed deviation in heading. The deviation is here the deviation from the original goal configuration. One example mission is illustrated in Fig. 4, where an autonomous vehicle, for example vehicle 1 in Fig.2, has the mission of parking in a parking space in a parking lot within configuration space 14 which is the whole parking lot. The parking spaces in the parking lot are delimited by road surface markings. The goal configuration in the parking space is defined as a goal point 16 in ^^ and ^^. The arrow 15 illustrates an optimal trajectory to the goal point 16. As a parking space has a larger size than most vehicles, it is possible to change the actual goal configuration without compromising the mission of parking within the road surface markings of the parking space. Fig.5 illustrates one example scenario when using the new RRT algorithm. The rigid line illustrates an optimal trajectory for vehicle 1, for example the trajectory of Fig.4. The black dot illustrates the original goal point at the end of the optimal trajectory. If another vehicle is parked in the parking space to the right of the parking space where the vehicle 1 wants to park, it will not be possible to take this optimal trajectory without colliding with the other vehicle. The constraint for the vehicle 1 imposed by the other vehicle is illustrated as a curvature constraint with a dashed line. Thus, it is not possible to reach the original goal point. The new RRT-algorithm is then generating new goal configurations, illustrated as a plurality of “x”. These new goal configurations are tried out one after one, and after a while some of them, denoted as bold “x”, are found to be able to be reached and thus considered feasible. In this case because they are placed beyond the curvature constraint. The other new goal configurations are considered unfeasible. With this kind of new RRT-algorithm, a new goal configuration is generated one or several times during the process of finding a feasible path and used in the searching. A new goal configuration is typically generated and used according to some kind of distribution rule. Below an “Algorithm 3” is shown to illustrate a general example of such a new RRT-algorithm: Algorithm 3: 1. G_init (^^^^^௧) 2. for ^^ ൌ 1 to ^^3. ^^^^^ௗ←RAND_CONF ( ) / / RAND_CONF_GOAL ( ) / / ^^^^^^4. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 5. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 6. ^^.add_vertex(^^^^௪^; 7. ^^.add_edge(^^^^^^, ^^^^௪^;8. Return ^^ In comparison with the Algorithm 1, the new Algorithm 3 additionally indicates use of a new random sampler in step 3, referred to as “goal random sampler” named as function RAND_CONF_GOAL in Algorithm 3 to indicate the new added feature of generating and using one or more new goal configurations in a general way. The goal random sampler is designed to generate one or more random new goal configuration within the vicinity of the original goal configuration. Vicinity here means a maximum allowed deviation from one or more of the characteristic parameters of the goal configuration, hence, a respective maximum allowed deviation from one or more of the goal configuration’s ^^, ^^, ^^, ^^ and ^^. “vicinity” may be defined as being a maximum allowed distance from the original goal configuration or being a defined area around the original goal configuration. The defined area may be represented by a shape, such as a square or rectangle and an allowed deviation from each edge or by a circle and an allowed deviation from the circle’s perimeter. The deviation, thus the maximum allowed distance, is for example, between 0.1 to 1 meter, for example 0.5 meter. The square, rectangle or circle may then define the original goal configuration, and the deviation is the maximum allowed distance from the original goal configuration. As previously described, the search is carried out in an iterative manner and the tree is in this process repeatedly expanded and becomes denser. Based on design preference, different alternatives of using one or more new goal configurations may be implemented and used. In other words, in some embodiments, the searching S7 comprises using the one or more new goal configuration based on a desired design of the RRT algorithm. The RRT-algorithm may be goal biased to speed up the expansion of the tree towards the goal configuration, for example as indicated in the goal-biased steps in Algorithm 2 as previously described. This is indicated in Algorithm 3 in that the goal configuration ^^^^^^can be used as an alternative to the random sampler function RAND_CONF. Hence, in a goal bias RRT-algorithm, the ^^^^^ௗsometimes becomes the goal configuration ^^^^^^. Hence, in some embodiment, the RRT-algorithm is goal-biased and searches the defined space in an iterative manner. The searching S7 comprises exchanging the random configuration for the goal configuration in a percentage of the iterations. In a goal-biased RRT-algorithm, the goal configuration ^^^^^^and thus the RAND_CONF may be exchanged for a new goal configuration generated with the RAND_CONF_GOAL function, as also indicated in step 3. The RRT-algorithm may then generally use the goal- biased steps in Algorithm 2 as previously described, however changed to also take new generated goal configurations generated with the RAND_CONF_GOAL function into account. The random configuration with the random sampler function RAND_CONF is then exchanged at one or several occasions for a goal configuration, the number of occasions for example expressed as a percentage of the iterations of the RRT-algorithm. Accordingly, the goal configuration ^^^^^^is exchanged for a new goal configuration generated with the RAND_CONF_GOAL function according to a distribution rule. In other words, in some embodiments, the searching S7 comprises exchanging the goal configuration for one of the one or more new goal configurations according to a distribution rule. The distribution rule may have different designs and may also change during the search as will be apparent from the example embodiments below. In one embodiment, the goal configuration is simply exchanged for a new goal configuration generated with the goal random sampler. The distribution rule is then that the goal configuration is always exchanged at the beginning of executing the RRT- algorithm. Below “Algorithm 3a” illustrates an example of such an RRT-algorithm: Algorithm 3a: 1. G_init (^^^^^௧) 2. ^^^^^^ ← RAND_CONF_GOAL ^ ^3. for ^^ ൌ 1 to ^^4. if ^^1 > ^^1^^^^then 5. ^^^^^ௗ←RAND_CONF ( ) 6. else 7. ^^^^^ௗ= ^^^^^^8. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 9. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 10. ^^.add_vertex(^^^^௪^; 11. ^^.add_edge(^^^^^^, ^^^^௪^;As indicated, the goal configuration is exchanged in step 2 before the iterations starts. Thereafter a distribution rule similar to the one in Algorithm 1a is used: ^^1^^^^is a first probability threshold (e.g., x=10 % becomes 0.1 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^1. If ^^1 > ^^1^^^^, a random configuration ^^^^^ௗis obtained by uniformly sampling in the configuration space using RAND_CONF, otherwise ^^^^^ௗis set to ^^^^^^. In other words, in some embodiments the distribution rule includes replacing S7a the goal configuration with the one or more new goal configurations. In another embodiment, ^^^^^ௗis set to a new goal configuration in a percentage of the cases that ^^^^^ௗshould be set to ^^^^^^. “Algorithm 3b” below is illustrating an example of such embodiment: Algorithm 3b: Build_RRT (^^^^^௧,^^,^^^^^ 1. G_init (^^^^^௧) 2. for ^^ ൌ 1 to ^^3. if ^^1 > ^^1^^^^^^^^then 4. ^^^^^ௗ←RAND_CONF ( ) 5. else 6. if ^^2 > ^^2^^^^7. ^^^^^ௗ= ^^^^^^8. else 9. ^^^^^ௗ←RAND_CONF_GOAL ( ) 10. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 11. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 12. ^^.add_vertex(^^^^௪^; 13. ^^.add_edge(^^^^^^, ^^^^௪^;14. Return ^^ In this example, ^^1^^^^is a first probability threshold (e.g., x=10 % becomes 0.1 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^1, according to a first distribution rule. If ^^1 > ^^1^^^^, a random configuration ^^^^^ௗis obtained by uniformly sampling in the configuration space using RAND_CONF, otherwise ^^^^^ௗis set to ^^^^^^or to a new goal configuration using function RAND_CONF_GOAL according to a second distribution rule. In this second distribution rule, ^^2^^^^is a second probability threshold (e.g., x=50 % becomes 0.5 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^2. In another embodiment, the goal configuration is exchanged for a new goal configuration generated with the goal random sampler when no feasible path can be found within a limit, defined e.g., as time or iterations. Below “Algorithm 3c” illustrates an example of such an RRT-algorithm: Algorithm 3c: Build_RRT (^^^^^௧,^^,^^^^^ 1. G_init (^^^^^௧) 2. for ^^ ൌ 1 to ^^3. if ^^1 > ^^1^^^^then 4. ^^^^^ௗ←RAND_CONF ( ) 5. else 6. if ^^ ^ ^^ or Solved7. ^^^^^ௗ= ^^^^^^8. else 9. ^^^^^ௗ, ^^^^^^←RAND_CONF_GOAL ( ) 10. m = m + Nbr; 11. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 12. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 13. ^^.add_vertex(^^^^௪^; 14. ^^.add_edge(^^^^^^, ^^^^௪^;15. Return ^^ In Algorithm 3c, a distribution rule similar to the one in Algorithm 1a is used: ^^1^^^^is a first probability threshold (e.g., x=10 % becomes 0.1 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^1. If ^^1 > ^^1^^^^, a random configuration ^^^^^ௗis obtained by uniformly sampling in the configuration space using RAND_CONF, otherwise another distribution rule is used where ^^^^^ௗis set to ^^^^^^if the number of iterations ^^ is smaller than a number ^^ or if ^^^^^^has been reached where ^^ is a subset of ^^. If the number of iterations ^^ is greater or equal to ^^ or if ^^^^^^has not been reached, a ^^^^^ௗis set to a new generated goal configuration generated by the goal random sampler and the present ^^^^^^is exchanged for the new generated goal configuration. ^^ is also updated with a number Nbr to update the limit for when ^^^^^ௗshall be exchanged for a new goal configuration. Nbr is a design parameter that is chosen based on design needs. In other words, in some embodiments the distribution rule comprises replacing S7b the goal configuration with one of the one or more new goal configurations upon failure to find a feasible path within expiration of an iteration limit or time limit. In one embodiment, the distribution rule includes to make the bias or weighting dependent on the distance to the goal configuration such that more new goal configurations are used closer to the goal configuration. This because it often becomes more difficult to find an exact solution closer to the goal configuration for a non-holonomic system. Below “Algorithm 3d” illustrates an example of such an RRT-algorithm: Algorithm 3d: Build_RRT (^^^^^௧,^^,^^^^^ 1. G_init (^^^^^௧) 2. for ^^ ൌ 1 to ^^3. if ^^1 > ^^1^^^^then 4. ^^^^^ௗ←RAND_CONF ( ) 5. else 6. if ^^2 > ^^2^^^^7. ^^^^^ௗ= ^^^^^^8. else 9. ^^^^^ௗ←RAND_CONF_GOAL ( ) 10. 11. ^^^^^^←NEAREST_VERTEX ( ^^^^^ௗ,^^ ) 12. ^^^^௪←NEW_CONF (^^^^^^,^^^^ ) 13. ^^.add_vertex(^^^^௪^; 14. ^^.add_edge(^^^^^^, ^^^^௪^;15. Return ^^ In this example, ^^1^^^^is a first probability threshold (e.g., x=10 % becomes 0.1 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^1, according to a first distribution rule. If ^^1 > ^^1^^^^, a random configuration ^^^^^ௗis obtained by uniformly sampling in the configuration space using RAND_CONF, otherwise ^^^^^ௗis set to ^^^^^^or to a new goal configuration using function RAND_CONF_GOAL according to a second distribution rule. In this second distribution rule, ^^2^^^^is a second probability threshold (e.g., x=50 % becomes 0.5 as probability threshold) and the variant uniformly samples in the interval [0, 1] to obtain a random probability value named ^^2. ^^2^^^^may be updated the closer the tree comes to the goal configuration, for example by increasing the ^^2^^^^with a value according to ^^2^^^^= ^^2^^^^+ value, such that an increasing number of goal configurations becomes a new goal configuration. It is here assumed that when a goal configuration is used, the tree comes closer to the goal configuration. In other words, in some embodiments, the distribution rule includes increasing S7c a number of replacements of the goal configuration with the one or more new goal configurations the closer the tree comes to the goal configuration. The bias, or weighting, then becomes dependent on the distance from the goal configuration the closer the tree is to the goal configuration by increasing the number of used new goal configurations. When the search is finished, a path through the tree ^^ is determined. Hence, the method comprises providing S8 a path for the autonomous vehicle using a result of the searching S7. A path is typically found by tracing back through the nodes from a feasible goal configuration to the starting configuration as exemplified in Algorithm 2. In other words, the method comprises providing S8 a path for the autonomous vehicle using a result of the searching S7 comprising tracing back from the goal configuration to the starting configuration along intermediate tree connections defining the determined path. The providing S8 may include determining the path based on the tracing back as explained. The providing S8 may include communicating the path to the navigation module whereupon vehicle 1 is operated according to the determined path. The navigation module may for example generate a trajectory for vehicle 1 to follow based on the determined path. The navigation module will then send control signals to one or more functions in vehicle 1 to follow the trajectory. The providing S8 may also include communicating the trajectory to a user via an interface arranged on vehicle 1, or via a mobile device, e.g., as a path on a map. Fig.6 illustrates a control arrangement 10 for providing a path of an autonomous vehicle 1 in a defined environment. The control arrangement 10 comprises control circuitry. Such control circuitry may comprise one or more electrical control units, ECUs, responsible for controlling one or more electrical systems or subsystems. The control arrangement 10 is herein generally explained as comprising processor 10a, memory 10b and a communication interface 10c. The control arrangement 10 is configured to store a computer program comprising instructions which, when the computer program is executed by a computer, cause the computer to carry out the method according to any one of the embodiments or examples as has been explained herein. The computer is for example comprised in the control arrangement 10. A computer may be any hardware or hardware / firmware device implemented using processing circuity such as, but not limited to, a processor, Central Processing Unit (CPU), a controller, an arithmetic logic unit (ALU), a digital signal processor, a microcomputer, a field programmable gate array (FPGA), a System-on-Chip (SoC), a programmable logic unit, a microprocessor, an application-specific integrated circuit, or any other device capable of electronically performing operations in a defined manner. The disclosure also relates to a computer- readable medium. The computer readable medium comprises instructions which, when executed by a computer, cause the computer to carry out the method according to any one of the embodiments or examples as has been explained herein. In some embodiments, the computer-readable medium may be a non-transitory computer-readable medium, such as a tangible electronic, magnetic, optical, infrared, electromagnetic, and / or semiconductor system, apparatus, and / or device. The control arrangement 10 includes, for example, a path planning module and a navigation module. The path planning module is responsible for generating a feasible path for the vehicle 1 based on a starting configuration and a goal configuration. The goal configuration may be received to the control arrangement 10 via an input module 14. For example, a user inputs an address or similar via a communication interface of the input module 14 and the input module communicates this address to the control arrangement 10. The control arrangement 10 may then translate the address into a goal configuration for the vehicle 1. The control arrangement 10 is thus configured to obtain a starting configuration and a goal configuration for the autonomous vehicle 1. Control arrangement 10 is further configured to search the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The search comprises to generate random configurations within the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle, and to determine a path for the autonomous vehicle 1 using a result of the search. The control arrangement 10 is further configured to search the defined environment comprising to generate one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler and use the one or more new goal configurations to find the feasible path with the RRT algorithm. The navigation module is responsible for operating the vehicle 1 according to the feasible path generated by the path planning module. The control arrangement 10 is arranged to receive sensor signals generated by one or more sensors 5, e.g., 5a ,5b, 5c attached to the vehicle 1 via the communication interface 10c, and to use such sensor signals for finding a feasible path. The control arrangement 10 is further arranged to output various control signals to functions of the vehicle 1 for operating the vehicle 1 along the feasible path, for example functions such as steering, forward driving, reverse driving, accelerating and braking. Such functions are generally denoted with a box and reference 12. The control arrangement 10 is further configured to output the result of the search and / or the found feasible path on a human-machine interface, HMI, 9. This HMI 9 may include, for example, one or more of a display unit, inpu / output device, and a processin unit. The input / output device may include one or more of a touchscreen, keyboard, buttons etc. The display unit may for example visualize the feasible path. In some embodiments, the disclosure relates to a computer readable medium storing instructions for executing the method as disclosed herein. In particular, storing instructions for executing a method for providing a path of an autonomous vehicle 1 in a defined environment, the autonomous vehicle 1 having certain dynamics and kinematics. The method comprising: obtaining S1 a starting configuration and a goal configuration for the autonomous vehicle and searching S7 the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration. The searching comprises generating random configurations in the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle, generating one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler, and using the one or more new goal configuration for finding the feasible path with the RRT algorithm. The method further comprises providing a path for the autonomous vehicle using a result of the searching. The computer program described herein may be a computer readable medium, and the computer program may be stored in a computer readable medium. The terminology used in the description of the embodiments as illustrated in the accompanying drawings is not intended to be limiting of the described method, control arrangement or computer program. Various changes, substitutions and / or alterations may be made, without departing from disclosure embodiments as defined by the appended claims. The term “or” as used herein, is to be interpreted as a mathematical OR, i.e., as an inclusive disjunction; not as a mathematical exclusive OR (XOR), unless expressly stated otherwise. In addition, the singular forms "a", "an" and "the" are to be interpreted as “at least one”, thus also possibly comprising a plurality of entities of the same kind, unless expressly stated otherwise. It will be further understood that the terms "includes", "comprises", "including" and / or "comprising", specifies the presence of stated features, actions, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, actions, integers, steps, operations, elements, components, and / or groups thereof. A single unit such as e.g. a processor may fulfil the functions of several items recited in the claims. The present disclosure is not limited to the above-described preferred embodiments. Various alternatives, modifications and equivalents may be used. Therefore, the above embodiments should not be taken as limiting the scope of the disclosure, which is defined by the appending claims.

Claims

Claims 1. A method for providing a path of an autonomous vehicle (1) in a defined environment, the autonomous vehicle (1) having certain dynamics and kinematics, the method comprising: - obtaining (S1) a starting configuration and a goal configuration for the autonomous vehicle; - searching (S7) the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration, wherein the searching comprises: o generating random configurations in the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle, o generating one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler, and o using the one or more new goal configuration for finding the feasible path with the RRT algorithm; and - providing (S8) a path for the autonomous vehicle using a result of the searching (S7).

2. The method according to claim 1, wherein the searching (S7) comprises using the one or more new goal configuration based on a desired design of the RRT algorithm.

3. The method according to claim 1 or 2, wherein the RRT-algorithm is goal-biased and searches the defined space in an iterative manner, wherein the searching (S7) comprises exchanging the random configuration for the goal configuration in a percentage of the iterations.

4. The method according to 3, wherein the searching (S7) comprises exchanging the goal configuration for one of the one or more new goal configurations according to a distribution rule.

5. The method according to 3 or 4, wherein the distribution rule includes one or more of:- replacing (S7a) the goal configuration with the one or more new goal configurations. - replacing (S7b) the goal configuration with one of the one or more new goal configurations upon failure to find a feasible path within expiration of an iteration limit or time limit; or - increasing (S7c) a number of replacements of the goal configuration with the one or more new goal configurations the closer the tree comes to the goal configuration.

6. The method according to any one of the preceding claims, wherein the defined environment includes road network and geospatial data.

7. The method according to any one of the preceding claims, comprising one or more of: - obtaining (S2) dynamics and kinematics of the autonomous vehicle (1); - obtaining (S3) a defined environment of the autonomous vehicle (1); - obtaining (S4) a step size of the RRT-algorithm and wherein the searching (S7) comprises using the step size for finding the feasible path with the RRT algorithm. - obtaining (S5) obstacle information and wherein the searching (S7) comprises using the obstacle information for finding the feasible path with the RRT algorithm. - obtaining (S6) real-time sensor data and wherein the searching (S7) comprises using the real-time sensor data for finding the feasible path from the starting configuration to the goal configuration.

8. The method according to any one of the preceding claims, wherein providing (S8) a path for the autonomous vehicle using a result of the searching (S7) comprises tracing back from the goal configuration to the starting configuration along intermediate tree connections defining the determined path.

9. A computer program comprising instructions which, when the computer program is executed by a computer, cause the computer to carry out the method according to any one of the preceding claims.

10. A computer-readable medium comprising instructions which, when executed by a computer, cause the computer to carry out the method according to any one of the preceding claims.

11. A control arrangement (10) for providing a path of an autonomous vehicle (1) in a defined environment, the autonomous vehicle (1) having certain dynamics and kinematics, wherein the control arrangement (10) is configured to: - obtain a starting configuration and a goal configuration for the autonomous vehicle (1); - search the defined environment using a Rapidly Exploring Random Tree, RRT, algorithm to find a feasible path from the starting configuration to the goal configuration, wherein the search comprises to: o generate random configurations within the defined environment using a random sampler taking into account driving constraints implied by the dynamics and kinematics of the autonomous vehicle, and to - determine a path for the autonomous vehicle (1) using a result of the search. characterized in that the control arrangement (10) further is configured to search the defined environment comprising to: - generate one or more new goal configurations in the vicinity of the goal configuration using a goal random sampler and use the one or more new goal configurations to find the feasible path with the RRT algorithm, 12. A vehicle (1) comprising the control arrangement (10) according to claim 11.

Images