# Underwater Vehicle Path Planning Based on Bidirectional Path and Cached Random Tree Star Algorithm
## 3 Bidirectional Cached Rapidly Exploring Random Tree Star
### Path-Caching Strategy  
In the intricate and dynamic milieu of the marine realm, dynamic or novel impediments may necessitate adept modifications to the random tree of the RRT* algorithm. A possible strategy is to promptly re-plan and abandon the original path when encountering previously unknown obstructions. The new route should evade current obstacles while maintaining the continuity of the original trajectory. To realize this continuity bias, a route cache must be established in the planning process. Path caching involves pre-saving nodes on the static plan's original path. In the novel path-planning process, these points have the opportunity to guide the growth of newly generated nodes by serving as reference  
Figure 2: Schematic diagram of two-way extended RRT* algorithm. \(T_{i}\) and \(T_{g}\) mean two random trees. \(q_{i}\) and \(q_{g}\) represent the node of random sampling expansion. points. Subsequently, under consideration of the impact of gravitational offset on the target position, the path cache functions as a directional tool guiding the growth of the random tree. An improved growth function \(F(x_{near})\) is proposed for generating new nodes from \(x_{near}\):  
\[F(x_{near})=R(x_{near})+G(x_{near})+P(x_{near}) \tag{1}\]  
where \(R(x_{near})\) represents the stochastic sampling operation performed on the nodes, \(G(x_{near})\) stands for the intended directional tendency function, and \(P(x_{near})\) signifies the cached path bias function. Furthermore, the gravitational influence exerted by the target vector \(x_{goal}\) on \(x_{near}\) can be mathematically formulated as:  
\[G=k_{g}\cdot\left\|x_{goal}-x_{near}\right\| \tag{2}\]  
The vector \(x_{goal}\) denotes the target point's position. The Euclidean norm of the difference between \(x_{goal}\) and \(x_{near}\), denoted by \(\left\|x_{goal}-x_{near}\right\|\), indicates the distance from the node to the target point. Furthermore, \(\rho\) signifies the search step length, while \(k_{p}\) stands for the gravitational coefficient. Consequently, the ensuing equation can be deduced as the objective bias function:  
\[G(x_{near})=\rho\cdot k_{g}\frac{x_{goal}-x_{near}}{\left\|x_{goal}-x_{near}\right\|} \tag{3}\]  
Analogously, the offset function for path caching can be formulated as follows:  
\[P(x_{near})=\rho\cdot k_{p}\frac{x_{path,i}-x_{near}}{\left\|x_{path,i}-x_{ near}\right\|} \tag{4}\]  
where \(k_{p}\) denotes the coefficient for path cache offset, and \(x_{path,i}\) signifies the positional vector of the \(i\)-th node within the pathway directory. Specify the Euclidean distance value between two points (fixed point and node) is denoted by \(\left\|x_{path,i}-x_{near}\right\|\) in the equation. The format of the random expansion function in the fundamental RRT* algorithm is represented as follows:  
\[R(x_{near})=\rho\cdot\frac{x_{rand}-x_{near}}{\left\|x_{rand}-x_{near}\right\|} \tag{5}\]  
By substituting Formulas (3)-(5) into Equation (1), a resultant expression is obtained:  
\[F(x_{near})=\rho\cdot\frac{x_{rand}-x_{near}}{\left\|x_{rand}-x_{near}\right\| }+\rho\cdot k_{g}\frac{x_{goal}-x_{near}}{\left\|x_{goal}-x_{near}\right\| }+\rho\cdot k_{p}\frac{x_{path,i}-x_{near}}{\left\|x_{path,i}-x_{near}\right\|} \tag{6}\]  
The algorithmic expression for generating the novel entity is additionally derived as follows:  
\[x_{new}=x_{near}+\rho\cdot\frac{x_{rand}-x_{near}}{\left\|x_{rand}-x_{near} \right\|}+\rho\cdot k_{g}\frac{x_{goal}-x_{near}}{\left\|x_{goal}-x_{near} \right\|}+\rho\cdot k_{p}\frac{x_{path,i}-x_{near}}{\left\|x_{path,i}-x_{near }\right\|} \tag{7}\]  
During implementation, three distinct probabilities are initially defined as \(p_{goal}\), \(p_{path}\), and \(p_{rand}\). \(p_{goal}\) represents the probability of selecting the target point (5), (6), or (7) as the new node to guide the growth of the random tree towards the goal. On the other hand, \(p_{path}\) corresponds to the probability of choosing a path cache point as a new node, while \(p_{rand}\) denotes the probability of selecting a new node at random.