# Improved Analytic Expansions in Hybrid A-Star Path Planning for Non-Holonomic Robots
## 3 An Improved Method  
The primary purpose of using the RS method in the analytic expansions is to increase the accuracy and speed up the search because the RS method is a well-known method for a non-holonomic vehicle to move from a configuration \([x_{0},y_{0},\theta_{0}]\) to another configuration \([x_{g},y_{g},\theta_{g}]\) in the shortest path. More details about RS curves can be found in [27]. Figure 3 depicts three different LSR routes using the RS method from an initial state \([x_{0},y_{0},\theta_{0}]=[0,0,0]\) to a goal state \([x_{g},y_{g},\theta_{g}]=[-3,5,\pi/2]\) according to three different values of curvature. The green route has the biggest curvature, but its length is the shortest of the three. Whereas the blue route corresponds to the smallest curvature, its length is the longest of the three. The turning radius is reversely proportional to curvature. The turning radius in the blue route is much larger than the radius in the orange and green routes. Therefore, it is possible to achieve a safer route for a car-like robot by adjusting curvature value. Figure 2: Original Hybrid A-star path planning in simulations. (**a**) Grid map A. (**b**) Grid map B. Among different paths generated from different curvatures, a better path is selected in terms of safety from a start state to a goal state. In order to evaluate each path, we propose the following objective function:  
\[G=\sigma_{1}\cdot v+\sigma_{2}\cdot m \tag{3}\]  
where, \(\sigma_{1}\) and \(\sigma_{2}\) are two coefficients normalizing the two related cost functions. The function \(v\) represents the cost of the Voronoi field, which is used in the post-processing Equation (2). The farther the robot is from obstacles, the smaller the value of the cost function. Utilizing this cost function, we see the robot prefers relatively equidistant moves to surrounding obstacles. Meanwhile, the function \(m\) is the cost of movement along the RS path which includes straight lines and curves as mentioned above. For a candidate RS path (\(p\)), costs of each straight line or curve are computed including its length (\(l_{i}\)), steering angle (\(s_{i}\)), and steer switching (\(c_{i}\)) in functions of (\(l_{p}\)), (\(s_{p}\)), and (\(c_{p}\)) respectively. With three constant weights \(w_{1},w_{2},w_{3}\), the cost function \(m\) has the following form:  
\[ \begin{split} m&=l_{p}+s_{p}+c_{p}\\ l_{p}&=w_{1}\cdot\sum l_{i}\\ s_{p}&=w_{2}\cdot\sum s_{i}\\ c_{p}&=w_{3}\cdot\sum c_{i}\end{split}  \tag{4}\]  
Because of the safety criteria, a car-like robot or a car does not always make the sharpest turn, or a maximum curvature, but it might choose a less sharp curvature to complete a turn. By combining these two sub-functions (\(v,m\)), with appropriate weights \(\sigma_{1}\), \(\sigma_{2}\), we trade off between selecting safer or faster path. If the coefficient \(\sigma_{1}\) is larger than \(\sigma_{2}\), then the robot prefers the safe path to the short one. The improved method in the analytic expansion is mainly divided into three steps:  
1. First, RS curves are generated based on different curvature values from the input state to the goal state. Collision to obstacles is checked along the curves. Hence the curve which does not collide with obstacles is chosen. 2. Second, the cost of each curve is calculated according to the Equation (3). 3. Third, the curve which has the lowest cost is selected as the best curve. The robot is expected to travel more safely along this curve. If such an RS curve exists, the method will assign it as the path to the goal and the search ends. If such a curve does not exist, the algorithm will continue with the forward search as in the original method:  
1. Computes robot continuous coordinates based on a non-holonomic model,
2. Converts these coordinates to corresponding discrete coordinates in the grid map,
3. Calculates objective costs according to the Equation (1),  
Figure 3: Reed-Shepp paths with different curvature values. at sharp corners.