## 4 Trajectory Optimization Problem
### Dynamically Feasible Trajectory  
To smooth the trajectory obtained after solving the optimization problems \(\mathcal{P}1\) or \(\mathcal{P}2\), we use state feedback control. It should be noted that the solutions to both \(\mathcal{P}1\) and \(\mathcal{P}2\) are motion primitives, so both can be used in the following trajectory smoothing technique based on a feedback control loop. The state feedback gain matrix is determined by solving a quadratic optimization problem in \(\mathcal{P}3\). To this end, each sampling interval \(h\), i.e., the point-to-point (p2p) trajectory, is discretized into \(\mathfrak{K}\) sub-sampling slots by sub-sampling time \(\delta\ll h\), where \(\mathfrak{K}=\lceil h/\delta\rceil\). From the trajectory optimizations \(\mathcal{P}1,2\) we obtain \(\mathfrak{U}[k],\ \forall k\in\mathcal{K}\). Then, for sub-sampling slots between \([k\sim(k+1)]h\), we define the following state transition model with state feedback gain matrix \(\Gamma\) (we consider \(\mathcal{D}\mathcal{K}\mathcal{M}2_{1}\) with subscript 2, e.g., \(X_{2}\) and \(\Gamma_{2}\)):  
\[X_{2}[kh+(\kappa+1)\delta]=\underbrace{(G_{2}-H_{2}\Gamma_{2}[kh+\kappa\delta ])}_{G_{2}^{\prime}}X_{2}[kh+\kappa\delta]+\left(\underbrace{H_{2}/\left( \frac{h}{\delta}\right)}_{H_{2}^{\prime}}\right)\mathfrak{U}[kh]; \tag{25}\]  
where \(\Gamma_{2}[kh+\kappa\delta]\in\mathbb{R}^{3\times 6}\) denotes the state feedback matrix, which is defined by solving the following quadratic optimization problem \((\mathcal{P}3)\):\[\mathcal{P}3:\] \[\begin{array}{ll}\text{OF3}.&\min_{\begin{subarray}{c}\Gamma_{2}[\kappa]\\ \forall k,\kappa\end{subarray}}\;\;(\;\omega_{3_{1}}\underbrace{\left\|X_{2}[ kh+(\kappa+1)\delta]-X_{2}^{dis}[kh+(\kappa+1)\delta]\right\|_{2}^{2}}_{\text{ OF3}}\\ &\qquad\qquad\qquad\qquad\qquad+\omega_{3_{1}}\underbrace{\left\|X_{2}[kh+( \kappa+1)\delta]-X_{2}[(k+2)h]\right\|_{2}^{2}}_{\text{OF3}})\\ \text{s. t.}&\text{C3}_{1}.&\text{C1}_{2-7},\forall k,\;\forall\kappa\in \mathfrak{K}.\end{array}\]  
where \(\omega_{3_{1}}\) and \(\omega_{3_{2}}\) are weighting coefficients, and \(X_{2}^{dis}[kh+(\kappa+1)\delta]\) denotes the state at step time \(kh+(\kappa+1)\delta\) on the original linearly discretized p2p trajectory between sampling time \([\,k\,\sim(k+1)]h\) (see Figure 4 for an illustration).  
OF3\({}_{1}\) acts as an elastic rubber band between the next state (e.g., assume the next waypoint) in the tentative smooth p2p trajectory and the next waypoint in the linearly discretized original p2p trajectory. In other words, OF3\({}_{1}\) ensures that the smooth p2p trajectory stays close to the original p2p trajectory by approaching (by minimizing the distance between) the next waypoint \(X_{2}[kh+(\kappa+1)\delta]\) of the p2p trajectory to the next waypoint \(X_{2}^{dis}[kh+(\kappa+1)\delta]\) of the linearly discretized original p2p trajectory. OF3\({}_{1}\), which acts as another rubber band, gives some flexibility to the p2p trajectory to be smooth by approaching (by minimization) the next waypoint \(X_{2}[kh+(\kappa+1)\delta]\) of the p2p trajectory to the second next waypoint \(X_{2}[(k+2)h]\) of the original trajectory. By tuning the weighting coefficients \(\omega_{3_{1,2}}\) (i.e., tuning of elasticity of rubber bands), the elasticity and smoothness of the method can be adjusted. The smoothing principle is illustrated in Figure 4 and presented in Algorithm 2.  
Figure 4: Illustration of the smoothing algorithm and concepts of the elasticity and smoothness of rubber bands.