CHAPTER 5: TRAJECTORY AND MOTION CONTROL
5.3. A-star algorithm for path finding
5.3.1. Introduction about Hybrid A-star algorithm
With the interface design and algorithm controlling from Section 5.2, demonstrating the capability to execute the trajectory movements from the given coordinates, we now move on to the next issue, which is finding a path for the autonomous vehicle from the predetermined path map. Now, the vehicle must autonomously calculate and find a suitable path while ensuring dynamic feasibility constraints.
The hybrid-state A-star, commonly referred to as hybrid A-star, is recognized as one of the most efficient path planners for non-holonomic autonomous vehicles. The initial deployment of this planner in outdoor environments occurred with the Stanford Racing Team's robot, Junior, during the 2007 DARPA Urban Challenge [16]. The hybrid A-star algorithm is a variant of the A-star algorithm [17]. The A-star algorithm is limited to expanding nodes in a discrete state space, which does not adhere to the dynamic feasibility constraints of the vehicle, as depicted in Figure 5.14(a). The hybrid A-star algorithm includes two phases (the forward search and analytic expansions) to produce an effective path [18].
(a) A-star search (b) Hybrid A-star search
Figure 5.14: Node expansion of A-star and hybrid A-star search algorithm.
In the forward search, akin to the conventional A-star algorithm, the hybrid A-star algorithm is guided by a cost-to-go function 𝑔(𝑛), representing the cost from the start node to the current node 𝑛,and a heuristic function ℎ1(𝑛).The heuristic function ℎ1(𝑛) utilizes the obstacle map to calculate the cost of the distance from the current node to the goal in the discrete state. Additionally, resembling an advanced version of the standard A-star, the hybrid A-star algorithm is also guided by another heuristic function ℎ2(𝑛) that disregards obstacles but accounts for the kinematics of a non-holonomic vehicle, such as autonomous vehicles.
The kinematics of the vehicle model are employed to predict motions based on the speed, direction, and steering angle of the vehicle in the continuous state. This approach aids the algorithm in selecting the right successor in continuous coordinates, which a non-holonomic vehicle is capable of following, as illustrated in Figure 5.14(b). These continuous coordinate successors are then approximately converted to discrete coordinate nodes to compute the total cost of a movement. Equation below represents the objective function of the planner at each
However, achieving the exact continuous coordinate goal node can be challenging because it depends on the resolution of the grid map and the smallest motion that the vehicle can make.
Analytic expansions constitute the second phase of the hybrid A-star algorithm, crucial for ensuring that the algorithm reaches the exact continuous coordinates of the goal state. In this phase, Reeds-Shepp (RS) [19] curves are employed, as the RS curves algorithm is a well- known method suitable for non-holonomic vehicles capable of both forward and backward movements. An RS curve is computed to generate the shortest path from a starting point to a goal point. Figure 5.15 illustrates a combination of Left, Straight, and Right curves (or LSR curve) between two configurations: [0, 0, 0] and [15, 2, −p/2], where the first two values represent position, and the third value denotes the orientation of the vehicle. The red arrow indicates the car's direction. The curvature value (0.5 in this case) allows the robot to achieve the maximum turning angle or minimum turning radius. The creators of the hybrid A-star claim that the analytic expansion method yields significant benefits in terms of accuracy and search speed [18].
Figure 5.15: Reeds-Shepp curve [18].
Additionally, a Voronoi field was proposed to assist the vehicle in navigating narrow openings by introducing a path-cost function as shown in Equation below:
When 𝑑0 ≥ 𝑑0𝑚𝑎𝑥, we have:
𝜌𝑣(𝑥, 𝑦) = 𝛼
𝛼+𝑑0(𝑥,𝑦)
𝑑𝑣(𝑥,𝑦)
𝑑0(𝑥,𝑦)+𝑑𝑣(𝑥,𝑦)(𝑑0−𝑑0𝑚𝑎𝑥
𝑑0𝑚𝑎𝑥 )2[16]
When 𝑑0 ≥ 𝑑0𝑚𝑎𝑥, 𝜌𝑣(𝑥, 𝑦) = 0
Where, 𝛼 is a constant falloff rate (𝛼 > 0), 𝑑0𝑚𝑎𝑥 is a constant maximum effective range of the field (𝑑0𝑚𝑎𝑥 > 0), 𝑑0 is the distance to the nearest obstacle (𝑑0 < 𝑑0𝑚𝑎𝑥), and 𝑑𝑣 is the distance to the nearest edge of the generalized Voronoi diagram. The effectiveness of this function lies in the fact that the cost is higher in the area near obstacles, decreasing to zero at points equidistant to obstacles. Consequently, the hybrid A-star path tends to be positioned in the center of the road.
5.3.2. Implementation hybrid A-start in project
In this self-driving car project, we employ the hybrid A-star algorithm as follows:
Firstly, we initiate the map creation. This map is essentially a Binary Occupancy Map, a 48x56 matrix with elements inside, where 1 represents obstacles and 0 represents drivable paths, with a resolution of 0.25 meters.
Secondly, we expand the influence range of obstacles. With the rationale of ensuring a minimum safe distance, obstacle areas are extended by 0.25 meters.
The next step involves utilizing the Hybrid A-star algorithm to determine the trajectory for the vehicle. The starting position is set at (28, 1), with the orientation angle of the vehicle being π/2. This corresponds to the center of the map as well as the target point. To optimize project implementation time, we use the “plannerHybridAstar” function within the Matlab application for efficient path computation.
Hybrid A-star Path Planner Algorithm with Two Waypoints 1 Algorithm HybridAStarPlanner(Map, Start, Goal)
2 Initialize Start node with (x, y, theta, g=0, h=heuristic(Start, Goal), f=g+h) 3 OpenSet ← {Start}
4 ClosedSet = ∅
5 while OpenSet is not empty do
6 current ← node in OpenSet with the lowest f-value 7 if current position equals Goal then
8 return ReconstructPath(current) 9 else Remove current from OpenSet 10 Add current to ClosedSet
11 for each neighbor of current generated by motion primitives 12 if neighbor is in ClosedSet then
13 continue to next neighbor 14 else
19 set f(neighbor) = g(neighbor) + h(neighbor) 20 if neighbor not in OpenSet
21 add neighbor to OpenSet 22 return Failure, no path found 23
24 function ReconstructPath(node) 25 path = []
26 while node is defined 27 path.prepend(node) 28 node = node.parent 29 return path
This detailed pseudocode outlines the Hybrid A* algorithm's flow:
- Initialization: The start node is initialized with its position, orientation, and costs.
- Main Loop: The algorithm continuously processes nodes from the open set, which are prioritized by the lowest f-value (g+h).
- Neighbor Expansion: For each current node, the algorithm generates neighbors based on valid motion primitives (actions the vehicle can take based on its dynamics).
- Cost Calculation: For each neighbor, it calculates the temporary g-value (cost from the start node), checks if this new path to the neighbor is better (lower cost) than any previously found path, and updates the costs accordingly.
- Path Construction: If the goal is reached, the path is reconstructed from the goal node to the start node by backtracking through the parent links of each node.
- Termination: The loop terminates when no nodes are left to process in the open set without finding a path, returning a failure. If a path is found, the detailed steps to reach the goal are returned.
This structure aims to give a clearer, step-by-step representation of the process, similar to traditional A* but adapted for the specific requirements of Hybrid A*, such as dealing with vehicle dynamics and continuous space.
Finally, we convert the coordinate arrays obtained from the aforementioned function into data to execute vehicle movements. Subsequently, these steps are iterated to continually compute new paths for the vehicle.