# Real-Time Hybrid Navigation System-Based Path Planning and Obstacle Avoidance for Mobile Robots
## 5 Trajectory Generation by Piecewise Cubic Bezier Curve
### C\({}^{2}\) Continuous PCBC  
To turn the Bezier curve path into a robot trajectory, the curve must meet robot kinematic constraints. Oscillated curves can make robots slip out of the desired trajectory. On the other hand, different from car-like robots, differential drive robot can track on C0 curve, but it is difficult to establish the controller for robot to track on it. The velocity and acceleration of robot must be continuous, thus, curve with continuous curvature meets 2 wheels differential drive mobile robot kinematic. Each curve segment is the distinct Bezier curve which has k+1 order polynomial function, i.e., each curve segment is C\({}_{k+1}\) function. The problem comes from the point that links 2 segments. The entire curve is undifferentiable at these points. Therefore, we must define Equations (13) and (14) to satisfy the kinematic constraint of robot.  
\[\dot{\textbf{B}}(1,v)=\dot{\textbf{B}}(0,v+1) \tag{13}\]  
\[\ddot{\textbf{B}}(1,v)=\ddot{\textbf{B}}(0,v+1) \tag{14}\]  
Cubic Bezier curve is the suitable solution because it has a 3 rd-order polynomial function and it is not-too-complicated. By setting each segment to be cubic Bezier curve, we have the matrix-form of entire PCBC that presented in (15).  
\[\textbf{B}(u,v)=\left[\begin{array}{cccc}u^{3}&u^{2}&u&1\end{array}\right] \left[\begin{array}{cccc}-1&3&-3&1\\ 3&-6&3&0\\ -3&3&0&0\\ 1&0&0&0\end{array}\right]\left[\begin{array}{c}\textbf{B}(0,v)\\ \textbf{C}_{1}(v)\\ \textbf{C}_{2}(v)\\ \textbf{B}(1,v)\end{array}\right] \tag{15}\]  
According to Equations (13) and (15) and (14) and (15), we have the Equations (16) and (17), respectively.  
\[C_{2}(v)=C_{1}(v+1) \tag{16}\]  
\[\textbf{C}_{1}(v)+4\textbf{C}_{1}(v+1)+\textbf{C}_{1}(v+2)=4\textbf{P}_{A}(v+1 )+2\textbf{P}_{A}(v+2) \tag{17}\]  
Equations (16) and (17) can be expanded into the system with n-2 equations and n variables. To constrain the last 2 variables \(\textbf{C}_{1}(0)\) and \(\textbf{C}_{2}(\text{n-2})\), Equations (18) and (19) are employed with \(\psi_{\text{A}}(0)\) and \(\psi_{\text{A}}(\text{n-1})\) are the initial angle of robot and destination angle, respectively.  
\[\textbf{C}_{1}(0)=\textbf{P}_{A}(0)+\left[\begin{array}{c}\cos(\psi_{A}(0)) \\ \sin(\psi_{A}(0))\end{array}\right]\frac{\|\textbf{P}_{A}(1)-\textbf{P}_{A}(0) \|}{3} \tag{18}\]  
\[\textbf{C}_{1}(n-1)=\textbf{P}_{A}(n-1)+\left[\begin{array}{c}\cos(\psi_{A} (n-1))\\ \sin(\psi_{A}(n-1))\end{array}\right]\frac{\|\textbf{P}_{A}(n-1)-\textbf{P}_{A} (n-2)\|}{3} \tag{19}\]  
Experimentally, \(\textbf{C}_{1}(0)\) and \(\textbf{C}_{1}(\text{n-1})\) determined by one-third of the Cartesian length between the corresponding point and the neighbor point in a significant point set. \(\textbf{C}_{1}(\text{n-1})\) is not used to construct the PCBC, but the insertion of it completes the system of Equations (16) and (17). The complete system of equation can be express as Equation (20).  
\[\left[\begin{array}{cccccccc}1&0&&&\cdots&&&0\\ 1&4&1&0&&&\vdots\\ 0&1&4&1&0&&&\\ \vdots&&&&\ddots&&&\\ &&&&0&1&4&1\\ 0&\cdots&&&&1&\vdots\\ \vdots&&&&\ddots&&&\\ &&&&0&1&4&1\\ 0&\cdots&&&&0&1&4&1\\ &&&&1&\vdots&\  
By solving the system of Equation (20), all control points can be obtained. The PCBC now has continuous curvature over the entire curve and only depends on the set of anchor points, start angle and destination angle. The significant point set in Figure 5c that fed to Equation (20) results in the continuous curvature over the entire path illustrated in Figure 6.  
The arc length of the \(v\)th segment given u = \(\mu\) can be found by Equation (21).  
\[l(\mu,v)=\int_{0}^{\mu}\|\dot{\mathbf{B}}(u,v)\|du \tag{21}\]  
The arc length of entire PCBC respect to curve parameters \(\mu\), \(v\) can be obtain by sum up total arc length of (\(\{v\}_{0}\)v-1)th curve segments and arc length of the \(v\)th curve segment given u=\(\mu\) that presented in Equation (22).  
\[L(\mu,v)=\sum_{v=0}^{v-1}l(1,v)+l(\mu,v) \tag{22}\]