I have a truck model with the following state space vector: $$ \underline{x} = \left[ \begin{array}{c} x\\ y\\ \theta\\ \alpha \end{array} \right] $$ $$u=\phi$$
Where the control input is the steering angle.
This system obeys the following differential equation: $$ \underline{\dot{x}} = \left[ \begin{array}{c} \cos(\theta)\\ \sin(\theta)\\ \frac{\tan(\phi)}{l}\\ -\left(\frac{\sin(\alpha)}{l_t}+\frac{\tan(\phi)}{l}\right) \end{array} \right] $$
I have a trajectory composed of $\underline{x_{i}} \quad \forall i=0,...,N$ where $\underline{x_{N}}$ is the goal configuration and $\underline{x_{0}}$ is the starting configuration. I want to smooth my current trajectory as it is very curvy but for doing so I need to develop the minimization algorithm, which currently looks like:
$$\min_{\phi_0,\dots \phi_N}\int_{0}^{N} \phi^2 \, d\phi$$ $$\text{subject to} \quad \mathbf{\dot{x}}=f(\underline{{x}},u)$$ $$|u| < 0.55 \quad \operatorname{rad}$$ $$\underline{x}[N]=\underline{x}_\text{goal}$$
And I want to keep the initial $\underline{x_{0}}$ as it is. Both the minimization function and the constraints are non-convex. The problem I face is that I do not know how to represent the system in order to plug it to a minimization tool as I want to use the current set of $\underline{x_{i}}$ as an initial guess and apply some kind of Gradient method to it in order to smooth the trajectory while keeping the constraints. I was thinking about linearizing the differential equation and plug the initial guess for the starting position but with this approach I would not consider the rest of current data points that I have. I would appreciate as well recommendations about tools that would help me to deal with this problem in the easiest way possible.
Figure depicts the current trajectory that I have where I would like to smooth some unnecessary turns done near the green object and to do that I would minimize the steering of the truck and keeping initial and final state vector as they originally are but I want to use my current trajectory (the whole) as a starting point and iteratively minimize on top of it.
In order to serve as an introduction to optimal control, we present a formulation as simple as possible, to the interpolation problem. In order to apply this algorithm, adequate discretization of the integral formulation will be necessary.
So regarding the interpolation problem
$$ \min J = \int_{t_i}^{t_f}\left(1+\frac{1}{2}\rho v^2\right)dt \ \ \text{s. t.}\ \ \left\{\begin{array}{rcl}\dot x & = & \cos\theta\\ \dot y & = & \sin\theta\\ \dot\theta & = & v\end{array}\right. $$
with $X(t) = (x(t),y(t),\theta(t)), \ X(t_i) = (x(t_i),y(t_t),\theta(t_i)), \ X(t_f) = (x(t_f),y(t_f),\theta(t_f))$ with $t_f$ unspecified, we can formulate the associated hamiltonian
$$ H = 1+\frac 12 \rho v^2 +\lambda_1\cos\theta+\lambda_2\sin\theta+\lambda_3 v $$
Here $\rho$ is introduced to guarantee that $|v|\le v_{max}$. The interpolation problem is solved as follows. (see)
$$ \cases{ \dot X = H_{\lambda }\\ \dot\lambda = -H_X\\ H_v = 0 } $$
Attached a MATHEMATICA script showing a particular interpolation