# Efficient Path Planning Based on Dynamic Bridging Rapidly Exploring Random Tree
## 4 Methodology
### Path Smoothing
#### 4.3.2 Candidate Node Sorting  
In this schematic, a succession of green dots marks candidate nodes. Priority is set for these adjacent candidate nodes. Every candidate node undergoes an evaluation, which includes measuring its local curvature and its proximity to the closest obstacle. According to our algorithm, candidate nodes with lower local curvature and further distance from obstacles will be assigned a higher priority.  
Moreover, the color of these candidate nodes also symbolizes their priority: the brighter the color, the higher the corresponding priority. Based on this priority, and given this hierarchy, these nodes are prioritized and integrated into the tree structure.  
Figure 5: The folded line is formed by joining vertices \(A_{1}\) and \(A_{2}\) to form two interior angles \(\alpha_{1}\) and \(\alpha_{2}\).  
As shown in Figure 6, the newly generated \(X_{new1}\) and \(X_{new2}\) will be inserted into the original path, providing more possibilities for path planning.  
For each reachable new node in the path, the angle between adjacent paths is calculated. For \(T_{start}\), the turning angle \(\alpha\) at the new node is composed of \(X_{new1}\overset{\rightarrow}{X_{nearest1}}\) and \(X_{new1}\overset{\rightarrow}{X_{new2}}\). For \(T_{goal}\), the turning angle \(\beta\) at the new node is composed of \(X_{new2}\overset{\rightarrow}{X_{new1}}\) and \(X_{new2}\overset{\rightarrow}{X_{nearest2}}\). The formula for calculating the \(T_{start}\) vector is shown in (7) and (8), and the formulas for calculating the steering angle \(\alpha\) and \(\beta\) are shown in (9) and (10).  
\[X_{new1}\overset{\rightarrow}{X_{nearest1}}=[X_{nearest}(x,y)-X_{new}(x,y)] \tag{6}\]  
\[X_{new2}\overset{\rightarrow}{X_{nearest2}}=[X_{nearest2}(x,y)-X_{new2}(x,y)] \tag{7}\]  
\[\alpha=ar\cos\left(\frac{X_{new1}\overset{\rightarrow}{X_{nearest1}}\cdot X _{new1}\overset{\rightarrow}{X_{new2}}}{\left\|X_{new1}\overset{\rightarrow} {X_{nearest1}}\right\|\left\|X_{new1}\overset{\rightarrow}{X_{new2}}\right\| }\right) \tag{8}\]  
\[\beta=ar\cos\left(\frac{X_{new2}\overset{\rightarrow}{X_{new1}}\cdot X_{new2 }\overset{\rightarrow}{X_{nearest2}}}{\left\|X_{new2}\overset{\rightarrow} {X_{new1}}\right\|\left\|X_{new2}\overset{\rightarrow}{X_{nearest2}} \right\|}\right) \tag{9}\]  
With a specified maximum handover constraint of \(\theta\) defined as \(0\leq\theta\leq 90\lx@math@degree\). For \(T_{start}\), when \(\alpha\geq\pi-\theta\), passes the handover constraint, a new node can be injected into \(T_{start}\). If \(0\leq\alpha<\pi-\theta\), the algorithm abandons the new node. Next, enter the extension of \(T_{goal}\), pass through when \(\beta\geq\pi-\theta\), and inject the new node into \(T_{goal}\).  
Employing this smoothing approach enables the generation of more fluid, superior-quality paths that adhere to the robot's motion stipulations. This method can not only improve the feasibility of the path, but also reduce the cost of controlling the robot during execution. In the following section, the effectiveness of the proposed method will be showcased across diverse different environments and scenarios. The following is the fast-smoothing pseudocode (Algorithm 4).  
Figure 6: Prioritized selection of candidate nodes. The nodes in both \(T_{start}\) and \(T_{goal}\) are represented as black nodes, and the green nodes are represented with different brightness according to their distance from the obstacle, the brighter the color, the higher the priority.