## 6 Continuous Trajectory Refinement  
The objective function consists of several subobjective functions: for improving the smoothness, for avoiding obstacles, and so forth. In this section, a precise explanation is given on how to construct subobjective functions for each of the various occasions. First, we examine the simplest case where only dynamic feasibility and obstacle avoidance constraints are taken into consideration. Let \(J\) be the objective function or performance index:  
\[J(\Gamma)=\xi_{smooth}J_{smooth}(\Gamma)+\xi_{obs}J_{obs}(\Gamma). \tag{41}\]  
There are various formulations of how \(J_{obs}\) and \(J_{smooth}\) are determined. In general, \(J_{smooth}\) can be expressed as:  
\[J_{smooth}(\Gamma)=\frac{1}{2}\int_{0}^{1}\left\|\frac{d\Gamma(t)}{dt}\right\| ^{2}dt. \tag{42}\]  
Eliminating unnecessary higher-order motion is the main objective of the \(J_{smooth}\). On the other hand, \(J_{obs}\) encourages to generate or modify collision-free trajectory by trying to push control points away from the obstacle zone if the trajectory is already in collisions or penalizing parts of the trajectory that is close to the obstacles. Let \(B\subset\mathbb{R}^{d}\) be the exterior boundary of the MAV and \(c\) is the cost function of penalizing close-in obstacles with respect to \(B\). Along with that, \(J_{obs}\) can be formulated as follows:  
\[J_{obs}(\Gamma)=\int_{0}^{1}\int_{u\in B}c(f_{c}(\Gamma(t),\mathbf{p}))\left\| \frac{df_{c}(\Gamma(t),\mathbf{p})}{dt}\right\|^{2}d\mathbf{p}dt, \tag{43}\]  
where the function \(f_{c}(\Gamma(t),\mathbf{p})\), which was proposed by [PERSON] at al. [57], can be defined as follows:  
\[f_{c}(\Gamma(t),\mathbf{p})=\left\{ \begin{array}{cl}-dis(\Gamma(t),\mathbf{p })+\frac{1}{2}\delta_{dis}&if\;dis(\Gamma(t),\mathbf{p})<0\\ \frac{1}{2\delta_{dis}}(dis(\Gamma(t),\mathbf{p})-\delta_{dis})^{2}&if\;0<dis( \Gamma(t),\mathbf{p})\leq\delta_{dis}\,,\\ 0&otherwise\end{array} \right. \tag{44}\]  
where \(\delta_{dis}\) denotes the distance from the boundary (\(B\)) of the quadrotor to a given obstacle position. Before taking gradient at \(i\), \(J(\Gamma)\) is linearized around \(i\), \(J(\Gamma)\approx J(\Gamma_{i})+(\Gamma-\Gamma_{i})^{T}\bigtriangledown J( \Gamma_{i})\). Defining \(c\) and \(d\) is detailed in [57] (Equations (22)-(28)). In [19], the cost of the trajectory was estimated based on the following formulation:  
\[J(\Gamma)=\xi_{obs}J_{obs}(\Gamma)+\xi_{smooth}J_{smooth}(\Gamma)+\xi_{soft}J_ {soft}(\Gamma),\quad J_{soft}(\Gamma)=J_{v}(\Gamma)+J_{a}(\Gamma), \tag{45}\]  
where \(J_{soft}(\Gamma)\) is determined by soft limits on acceleration and velocity. \(J_{smooth}(\Gamma)\) is defined by considering only geometric information without minimizing snap and/or jerk [53]. Such minimization is required because of the following stages of trajectory optimization. In such trajectory optimization, time reallocation has less impact on the objective function. Hence, \(J_{smooth}(\Gamma)\) is defined as follows:  
\[J_{smooth}(\Gamma)=\Sigma_{i=d-1}^{n+1-d}\left\|\underbrace{\mathbf{p}_{i+1 }-\mathbf{p}_{i}}_{f_{i+1,i}}+\underbrace{\mathbf{p}_{i-1}-\mathbf{p}_{i}}_{f _{i-1,i}}\right\|^{2}, \tag{46}\]  
where a number of control points, denoted \(\mathbf{n}\), and \(\mathbf{f}_{i+1,i}\) and \(\mathbf{f}_{i-1,i}\) can be interpreted as connecting joint force of two springs between control points pairs: \((\mathbf{p}_{i+1},\mathbf{p}_{i})\) and \((\mathbf{p}_{i-1},\mathbf{p}_{i})\), for example, control points lie on a straight line if the sum of all terms equals zero. As an aside, similar approaches were proposed in [85; 86]. The value of \(J_{obs}(\Gamma)\) is determined by calculating the distance to the closest object pose from each control point, in which the distance to the obstacle, i.e., \(f_{c}(\mathbf{p}_{i})\), is given by:\[f_{c}(\mathbf{p}_{i})=\left\{ \begin{matrix}(dis(\mathbf{p}_{i})-\delta)^{2}&dis( \mathbf{p}_{i})\leq\delta_{dis}\\ 0&dis(\mathbf{p}_{i})>\delta_{dis}\end{matrix} ,\right.