# A Two-Stage Local Path Planning Algorithm Based on Sampling and Optimization Methods
## 3 Proposed Method
### Exploration Stage
#### 3.2.2 Candidate Paths  
The candidate paths are a set of paths parallel to the global path and free from collisions with obstacles. These candidate paths are generated using the lateral planning and the diagonal planning, as illustrated in Figure 4.  
In lateral planning, the robot is first sampled at a constant velocity. Assuming the robot moves forward in a straight line at the maximum velocity \(v_{max}\) within a specified time interval \(\Delta t\), the maximum distance it travels is \(S_{max}\). This allows us to generate a series of discrete position points, forming a lateral path. Next, positions are sampled on both sides of the global path at a fixed width \(W\). Assume that \(N\) paths are sampled on each side. Consequently, a total of \(2\times N\) lateral paths can be generated on both sides of the global path. The formula for generating the \(n\)th lateral path is given as  
\[j_{max}=\frac{S_{max}}{v_{max}\cdot\Delta t} \tag{3}\]  
\[\{Hs_{j}^{n},Hd_{j}^{n}\}=(s_{0}+j\cdot v_{max}\cdot\Delta t,W\cdot(n-N)),\ \ j \in(1,j_{max}) \tag{4}\]  
Figure 4: The process of obtaining candidate paths.  
where \((s_{0},d_{0})\) in (4) is the coordinate of the robot in the planning coordinate system. \(\{Hs_{j}^{n},Hd_{j}^{n}\}\) is the position set of the \(n\)th lateral path. \(j\) represents the position point index of the lateral path, and \(j_{max}\) in (3) is the total number of position points in the lateral planning.  
At the same time, there is a diagonal path between the robot and the first sampling point in each lateral path. Therefore, the diagonal planning needs to be performed. The width \(R\) of the robot is used as the distance sampling value. Thus, a diagonal path between the robot and the \(n\)th lateral path is generated. The formula for generating the \(n\)th diagonal path is expressed as  
\[\alpha=arctan\frac{Hd_{1}^{n}-d_{0}}{Hs_{1}^{n}-s_{0}} \tag{5}\]  
\[k_{max}=\|(Hs_{1}^{n},Hd_{1}^{n})-(s_{0},d_{0})\|/R \tag{6}\]  
\[\{Ds_{k}^{n},Dd_{k}^{n}\}=(s_{0}+k\cdot cos\alpha\cdot R,d_{0}+k\cdot sin\alpha \cdot R),\ \ k\in(1,k_{max}) \tag{7}\]  
where \(\alpha\) in (5) is the intermediate variable. \(\{Ds_{k}^{n},Dd_{k}^{n}\}\) in (7) is the position set of the \(n\)th diagonal path. \(k\) represents the position point index of the diagonal path, and \(k_{max}\) in (6) is the total number of position points of the diagonal planning.  
The lateral path position sets \(\{Hs_{j}^{n},Hd_{j}^{n}\}\) and the diagonal path position sets \(\{Ds_{k}^{n},Dd_{k}^{n}\}\) together form the generation path sets \(\{Gs_{m}^{n},Gd_{m}^{n}\}\), as illustrated by the dashed trajectories in Figure 4.  
Next, the obstacle collision detection needs to be performed on the generation path sets, where the paths that collide with obstacles are discarded. The discarded paths can be determined as  
\[m_{max}=j_{max}+k_{max} \tag{8}\]  
\[\|\{Gs_{m}^{n},Gd_{m}^{n}\}-(Ls_{i},Ld_{i})\|<R,\ \ m\in(1,m_{max}) \tag{9}\]  
If any position of the \(n\)th generation path satisfies the above equation, that path is considered a discarded path, depicted as the gray dashed trajectories in Figure 4. The lateral paths remaining in the final set of generation paths are considered candidate paths \(\{Cs_{m}^{n},Cd_{m}^{n}\}\), illustrated as the yellow and red dashed trajectories in Figure 4.