## 3 RPM Path Planning Method Based on Probabilistic Roadmap and Heuristic Rule
### RPM for Solving the Optimal Control Path Planning Problem  
To solve the optimal control problem corresponding to Equations (5)-(11), the Radau pseudospectral method is used to discretize the continuous-time optimal path planning problem into a finite-dimensional NLP problem to solve for the optimal time of the cabin pushing control. First, the time interval of the problem needs to be normalized, i.e., the time interval \(\left[t_{0},t_{f}\right]\) is transformed into \((-1,1]\) that can be solved by pseudospectral, as in Equation (12). \[\tau=\frac{2t}{t_{f}-t_{0}}-\frac{t_{f}+t_{0}}{t_{f}-t_{0}}=\frac{2t-t_{f}-t_{0} }{t_{f}-t_{0}} \tag{12}\]  
In Equation (12), \(\tau\) is the normalized time, \(t\in\left[t_{0},t_{f}\right]\). In order to solve the problem, the state variables, control variables, constraint functions, optimization objective functions, etc., need to be discretely varied. Solving with the RPM requires dividing the time interval. However, the collocation point taken by the RPM is the Legendre Gauss Radau (LGR) point, which takes the value interval \((-1,1]\), excluding the left endpoint of the interval. To be consistent with the time interval required by the RPM, a discrete point is added, i.e., the total number of discrete points is \(N+1\) and the interval is \([-1,1]\). Figure 3: Area-based obstacle avoidance constraints. The LGR point of order \(N\) is the solution of the polynomial \(P_{N}(\tau)+P_{N-1}(\tau)=0\), where \(P_{N}(\tau)\) denotes the Legendre polynomial of order \(N\):  
\[P_{N}(\tau)=\frac{1}{2^{N}N!}\cdot\frac{d^{N}}{d\tau^{N}}\bigg{(}\left(\tau^{2 }-1\right)^{N}\bigg{)} \tag{13}\]  
The set of LGR collocation points using Equation (13) is \(\{\tau_{1},\tau_{2},\ldots,\tau_{N}\}\). Further, the state variables \(\{X_{1},X_{2},\ldots,X_{N}\}\) and the control variables \(\{U_{1},U_{2},\ldots,U_{N}\}\) at the LGR collocation points can be obtained. Then, the state variable \(x(\tau)\) and control variable \(u(\tau)\) can be expressed using Lagrange polynomials:  
\[x(\tau)\approx X(\tau)=\sum_{i=0}^{N}L_{i}(\tau)X(\tau_{i}) \tag{14}\]  
\[u(\tau)\approx U(\tau)=\sum_{i=1}^{N}L_{i}(\tau)U(\tau_{i}) \tag{15}\]  
Since the point \(\tau_{0}=-1\) is added at the interval \((-1,1]\), in the state variable approximation in Equation (14), \(i=0\). The control variable approximation uses only LGR points for approximation, excluding the \(-1\) point, so \(i=1\) in Equation (15). In Equations (14) and (15), \(L_{i}(\tau)\) represents the Lagrange interpolation basis function, expressed as:  
\[L_{i}(\tau)=\prod_{j\
eq i,i=1}^{N}\frac{\tau-\tau_{j}}{\tau_{i}-\tau_{j}} \tag{16}\]  
in which,  
\[L_{i}\big{(}\tau_{j}\big{)}=\left\{ \begin{array}{l}1,\;i=j\\ 0,i\
eq j\end{array} \right. \tag{17}\]  
Further, the constraints are transformed. The derivative of the approximate state variable is obtained by deriving the derivative of the approximate state variable obtained from the Lagrange interpolation polynomial:  
\[\dot{x}(\tau_{i})\approx X(\tau_{i})=\sum_{j=0}^{N}\dot{L}_{j}(\tau_{i})X_{j} =\sum_{j=0}^{N}D_{ij}X_{j} \tag{18}\]  
where \(D_{ij}\) is the differential matrix and the expression is:  
\[D_{ij}=\left\{ \begin{array}{l}\frac{\dot{x}(\tau_{i})}{(\tau_{i}-\tau)\dot{ x}(\tau)},if\;i\
eq j\\ \frac{\ddot{x}(\tau_{i})}{2\dot{x}(\tau_{i})}\qquad,if\;i=j\end{array} \right. \tag{19}\]  
In Equation (19), \(g(\tau_{i})=(1+\tau_{i})[P_{N}(\tau_{i})-P_{N-1}(\tau_{i})]\), \(\dot{g}(\tau_{i})\) is the first-order derivative and \(\ddot{g}(\tau_{i})\) is the second-order derivative. The differential equation constraint can be transformed into:  
\[\sum_{i=0}^{N}D_{ij}X\big{(}\tau_{j}\big{)}=\frac{t_{f}-t_{0}}{2}f(X(\tau_{i}),U(\tau_{i}),\tau_{i}) \tag{20}\]  
In addition, for the path planning problem, it is necessary to determine the terminal state \(x\Big{(}\tau_{f}\Big{)}\) of the PMCU at the target location.