## 3 Trajectory Planning
### Trajectory Planning
\begin{split}&\int\left(1+\frac{\beta^{2}(\sqrt{2}-2)\left(8-32 \gamma+32\gamma^{2}\right)-V_{B}}{\left[\alpha+4\beta\gamma(1-\gamma)\right] \left[\alpha-4\beta\gamma(1-\gamma)\right]}\right)\mathrm{d}\gamma\\ &=\int\left(\frac{\varepsilon}{\left[\alpha-4\beta\gamma(1- \gamma)\right]}+\frac{\xi}{\left[\alpha+4\beta\gamma(1-\gamma)\right]}+1 \right)\mathrm{d}\gamma\\ &=\gamma+\frac{\xi}{4\beta\delta}\ln\left(\frac{\gamma-0.5+0.5 \delta}{0.5+0.5\delta-\gamma}\right)\\ &+\frac{\varepsilon}{2\beta\eta}\arctan\left(\frac{2\gamma-1}{ \eta}\right),\end{split}  \tag{23}\]where \(\delta=\sqrt{\left(\sqrt{\frac{V_{B}}{V_{B}-V_{N}}}+1\right)}\), \(\eta=\sqrt{\left(\sqrt{\frac{V_{B}}{V_{B}-V_{N}}}-1\right)}\), \(\xi=(4\sqrt{2}-8)(\chi+\beta)-\frac{\kappa}{2}\), \(\varepsilon=(4\sqrt{2}-8)(\chi-\beta)-\frac{\kappa}{2}\). Hence \(\gamma_{i}\) can be obtained by solving the root of the following formula  
\[ \begin{split}&\int_{0}^{\gamma_{i}}\left(1+\frac{\beta^{2}( \sqrt{2}-2)\left(8-32\gamma+32\gamma^{2}\right)-V_{B}}{\left[\alpha+4\beta\gamma (1-\gamma)\right]\left[\alpha-4\beta\gamma(1-\gamma)\right]}\right)\mathrm{d} \gamma\\ &=\gamma_{i}+\frac{\xi}{4\beta\delta}\ln\frac{\xi+2(\delta+1) \gamma_{i}}{\xi-2(\delta-1)\gamma_{i}}\\ &+\frac{\varepsilon}{2\beta\eta}(\arctan\frac{2\gamma_{i}-1}{ \eta}+\arctan\frac{1}{\eta})\\ &=\frac{8(5-2\sqrt{2})i\Delta t((V_{N}-V_{B}))}{15m},\end{split}  \tag{24}\]  
where \(\xi=\sqrt{\frac{V_{B}}{V_{B}-V_{N}}}\). The function \(F(\gamma_{i})\) can be defined as,  
\[ \begin{split} F(\gamma_{i})&=\frac{1}{(V_{N}-V_{B} )}\gamma_{i}+\frac{\xi}{4\beta\delta(V_{N}-V_{B})}\ln\frac{\xi+2(\delta+1) \gamma_{i}}{\xi-2(\delta-1)\gamma_{i}}\\ &+\frac{\varepsilon}{2\beta\eta(V_{N}-V_{B})}(\arctan\frac{2 \gamma_{i}-1}{\eta}+\arctan\frac{1}{\eta})-\frac{8(5-2\sqrt{2})i\Delta t}{15 m}.\end{split}  \tag{25}\]  
The derivative of the function \(F(\gamma_{i})\) is,  
\[F^{\prime}(\gamma_{i})=\frac{1}{(V_{N}-V_{B})}\frac{-16(V_{B}-V_{N}){(\gamma_ {i}-{\gamma_{i}}^{2})}^{2}+S}{\left[V_{B}-16(V_{B}-V_{N}){(\gamma_{i}-{\gamma _{i}}^{2})}^{2}\right]}, \tag{26}\]  
where \(S=32(2-\sqrt{2})(V_{N}-V_{B})\gamma^{2}-32(2-\sqrt{2})(V_{N}-V_{B})\gamma+8( 2-\sqrt{2})(V_{N}-V_{B})\). Due to that \(\gamma_{i}\in[0,1]\) and \(V_{B}>V_{N}\), the following formula \(-16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}\leq 0\) holds. When \(\gamma_{i}=-\frac{-32(2-\sqrt{2})(V_{N}-V_{B})}{64(2-\sqrt{2})(V_{N}-V_{B})}=0.5\), \(S\) takes the maximum value \(0\), as a result, \(S\leq 0\), \(-16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}+S<0\), and the molecule of the derivative \(F^{\prime}(\gamma_{i})\) is less than \(0\). When \(\gamma_{i}=0.5\), \(16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}\) has the maximum value \(V_{B}-V_{N}\), so \(16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}\leq V_{B}-V_{N}\), therefore, \(-16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}\geq V_{N}-V_{B}\), as a result, \(V_{B}-16(V_{B}-V_{N})(\gamma_{i}-{\gamma_{i}}^{2})^{2}\geq V_{B}+V_{N}-V_{B}= V_{N}>0\). As \(V_{N}-V_{B}<0\), the denominator of the derivative \(F^{\prime}(\gamma_{i})\) is less than zero. That is, \(F^{\prime}(\gamma_{i})>0\). Therefore, the function \(F(\gamma_{i})\) is monotonically increasing and has a unique solution. The Newton-Raphson iterations are used as follows, so as to meet the requirements of real-time calculation. \[ \begin{split}\gamma_{i}^{(r)}&=\gamma_{i}^{(r-1)}- \frac{F(\gamma_{i}^{(r-1)})}{F^{\prime}(\gamma_{i}^{(r-1)})},\ \ \ \ \ r=1,2,...\\ \gamma_{i}^{(0)}&=\gamma_{i-1}+\frac{v_{2}(\gamma_{i -1})}{\sigma(\gamma_{i-1})}\Delta t.\end{split}  \tag{27}\]  
Generally, the accuracy of the robot can be achieved in just \(1\) or \(2\) iterations. Hence the total time \(T_{2}\) in this segment is \[