# Research on the Path Planning of Unmanned Sweepers Based on a Fusion Algorithm
## 3 Method
### Path Smoothing Based on Minimum Snap  
The path searched by A* algorithm does not consider the kinematic model, so there will be unsmooth points at the corner. The robot does not have a motion mutation at one point. If it is a universal wheel robot, it also needs to rotate at a certain angle to continue forward, which obviously wastes a lot of time and efficiency. Therefore, based on the track points in the track, a smooth curve connecting each track point is planned. This paper samples the trajectory smoothing processing based on minimum snap. It is difficult to express a complex trajectory with only one polynomial, so we divide the trajectory into multiple segments according to time, and each segment is expressed using a polynomial curve, each with the following equation:  
\[p(t)=\left\{ \begin{array}{ll}\begin{bmatrix}1,t,t^{2},\ldots,t^{n}\\ 1,t,t^{2},\ldots,t^{n}\\ \end{bmatrix}\cdot p_{1}&t_{0}\leq t\leq t_{1}\\ \begin{bmatrix}1,t,t^{2},\ldots,t^{n}\\ \end{bmatrix}\cdot p_{2}&t_{1}\leq t\leq t_{2}\\ \begin{bmatrix}1,t,t^{2},\ldots,t^{n}\\ \end{bmatrix}\cdot p_{3}&t_{2}\leq t\leq t_{3}\\ \ldots\\ \begin{bmatrix}1,t,t^{2},\ldots,t^{n}\\ \end{bmatrix}\cdot p_{k}&t_{k-1}\leq t\leq t_{k}\\ \end{array} \right. \tag{5}\]  
Figure 3: Key point extraction diagram. (The red square is the starting point, the blue square and the yellow square are the characteristic points in the path, and the green square is the ending point). \(K\) is the number of segments of the trajectory. \(p_{i}=[p_{i0},p_{i1},p_{i2},\ldots,p_{in}]^{T}\) is the parameter vector of the i-th segment trajectory. We want smoothness (position and velocity, etc.) at adjacent trajectories. Usually, there are many trajectories that satisfy the constraints, but we only need one specific trajectory; therefore we need to construct an optimal function to find the optimal trajectory among the feasible trajectories. Thus, we modelled the problem as a constrained optimization problem, as in Equation (6), below. \[\left\{ \begin{aligned} & minf(p)\\ & s.t.~{}~{}~{}A_{eq}p=b_{eq}\\ & A_{iaq}p\leq b_{iaq}\end{aligned} \right. \tag{6}\]  
In Equation (6), \(p\) denotes the trajectory parameter; \(s.t.\) means constraint, meaning subject to. All we had to do is to list the \(A_{eq}\), \(b_{eq}\), \(A_{iaq}\), bioel parameters from the optimization problem and put them into the optimizer (a library in Matlab) to solve for the trajectory parameter \(p\). The target trajectory parameter \(p\) was solved using the optimization method. First, according to the path point, the trajectory was divided into k segments, the distance of each segment calculated, according to the distance equalization time \(T\), to obtain the time series \(t_{0},~{}t_{1},~{}t_{2},\ldots,t_{k}\). The time allocation assigned the total time T to each segment according to the distance of each segment. Then, the optimization function was constructed. The optimization function for minimum snap is shown in Equation (7), Equations (8) and (9) is the solution for \(Q_{i}\). \[\min\int_{0}^{T}\Big{(}P^{(4)}(t)\Big{)}^{2}dt=min\sum_{i=1}^{k}\int_{t_{i-1}}^ {t_{i}}\Big{(}P^{(4)}(t)\Big{)}^{2}dt=min\sum_{i=1}^{k}p^{T}Q_{i}p \tag{7}\]  
\[Q_{i}=\int_{t_{i-1}}^{t_{i}}\bigg{[}0,0,0,24,\ldots,\frac{n!}{(n-4)!}t^{n-4} \bigg{]}^{T}\bigg{[}0,0,0,24,\ldots,\frac{n!}{(n-4)!}t^{n-4}\bigg{]}dt \tag{8}\]  
\[Q_{i}= \begin{bmatrix}0_{4\times 4}\\ 0_{(n-3)\times 4}\end{bmatrix} \frac{0_{4\times(n-3)}}{(c-4)!}\frac{c!}{(c-4)!} \frac{1}{(r-4)+(c-4)+1}\Big{(}t_{i}^{(r+c-7)}-t_{i-1}^{(r+c-7)}\Big{)}\bigg{]} \tag{9}\]  
where \(r\) and \(c\) are the row indexes and column indexes of the matrix, with indexes starting from 0 and the first row \(r=0\); \(p^{(4)}(t)\) represents the 4 th-order derivative with respect to time \(t\). \[\text{Q}= \begin{bmatrix}Q1&0&0\\ 0&\ddots&0\\ 0&0&Qn\end{bmatrix}