# Research on Lazy Theta+
## 5 B-Spline Route Smoothing
### B-Spline Equally Spaced Sampling  
The equidistant sampling point sequence of the B-spline curve is more suitable for aircraft route planning, as it is a polyline according to the connection of the uniform sampling points of B-spline about \(t\), and accumulates the length of the line segment along the polyline. When the length is close to the sampling interval, the equidistant sampling method uses the binary method to search for a point so that the point reaches the previous sampling point. The distance is equal to the sampling spacing [45].  
The calculation steps are as follows.  
1. According to B-spline uniform sampling, a uniform sampling point sequence about parameter \(t\) is obtained, denoted as \(\{p(t_{m})\}_{m=1}^{M}=\{p(x(t_{m}),y(t_{m}),z(t_{m}))\}_{m=1}^{M}\), where \(p(t_{m})\triangleq p(x(t_{m}),y(t_{m}),z(t_{m}))\) is the \(m\)-th sampling point about the parameter \(t\), \(t_{m}=\frac{(m-1)\cdot(N-K+1)}{M}\), and \(M\gg N-K\).
2. Let point \(P_{d}\triangleq p(t_{1})=p(x(t_{1}),y(t_{1}),z(t_{1}))\), and put \(p_{d}\) into \(S_{d,s}\), where \(p_{d}\) is the first sampling point. Let \(m_{a}=1\) be used to mark the sampling point \(p(t_{m_{a}})\) in \(S_{t}\), and \(m_{a}\) is used for the iterative process of the dichotomy.
3. In \(S_{t}\), starting from point \(p(t_{m_{a}})\), find the next sampling point \(p(t_{m_{a}})\), which is required to satisfy Equation (27).  
\[m_{a}^{\prime}\triangleq\operatorname{argmin}_{m}\bigl{\{}m\bigl{|}d_{\Sigma }(p_{d},p(t_{m}))\geq d,m_{a}\leq m\leq M\bigr{\}} \tag{27}\]  
where \(d_{\Sigma}(,)\) represents the cumulative distance and \(d_{\Sigma}(p_{d},p(t_{m}))\) is the path length from point \(p_{d}\) to point \(p(t_{m})\) along the polyline specified by \(S_{t}\).  
1. If any point \(p(t_{m})\) has \(d_{\Sigma}(p_{d},p(t_{m}))<d(m_{a}\leq m\leq M)\), then: let \(m_{a}^{\prime}=M\), \(p_{d}=p(t_{M})\); put point \(p_{d}\) into \(S_{d,s}\); point \(p_{d}\) is the last sampling point of \(S_{d,s}\); and move to step 5.
2. If \(m_{a}^{\prime}=M\), the last point of \(S_{t}  
3. If \(m_{a}^{\prime}<M\), then \(d_{\sum}(p_{d},p(t_{m_{a}^{\prime}-1}))<d\), meaning that the point whose cumulative distance to point \(p_{d}\) is \(d\) is located on \(p(t_{m_{a}^{\prime}})p(t_{m_{a}^{\prime}+1})\). Move to step 4 to search for this point by dichotomy.
4. On the line segment \(p(t_{m_{a}^{\prime}})p(t_{m_{a}^{\prime}+1})\), use the dichotomy to find the point \(p(\overline{t})\), such that \(d_{\sum}(p_{d},p(\overline{t}))\approx d\) or \(|d_{\sum}(p_{d},p(\overline{t}))-d|\leq\varepsilon\), where \(\varepsilon\) is the sampling spacing error, which is used to control the search accuracy of the dichotomy. Let \(p_{d}=p(\overline{t})\), put \(p_{d}\) into \(S_{d,s}\) as a new sampling point, and let \(m_{a}=m_{a}^{\prime}\). Move to step 3 to continue execution.
5. Obtain \(S_{d,s}\) by calculation; that is, the sequence of the equally spaced sampling points of the B-spline route.  
Figure 15 is a schematic diagram of the accumulated distance, and Figure 16 is a schematic diagram of a smooth route with equal spacing sampling, where the red squares represent the control points and the black dots represent the sampling points