I have a dynamical system with the following state vector and control input:
$$ \underline{x} = \left[ \begin{array}{c} x \\ y \\ \theta \end{array} \right] $$ $$u=\kappa$$
This system obeys the following differential equation:
$$ \underline{\dot{x}} = \left[ \begin{array}{c} \cos(\theta) \\ \sin(\theta) \\ \kappa \end{array} \right] $$ My objective is to go from an initial pose to a final pose while minimizing a determined cost function:
$$\min_{\kappa(t)}\int_{0}^{T} \kappa(t)^2 \, dt$$ $$\text{subject to} \quad \mathbf{\dot{x}}=f(\underline{{x}},u)$$ $$|u| < 0.12 \quad \operatorname{rad/s}$$ $$\underline{x}[0]=\underline{x}_\text{initial}$$ $$\underline{x}[N]=\underline{x}_\text{goal}$$
In this question they provided me with an interpolation problem to go from an initial pose to a goal pose, but in my case it has been given to me an array of N discretized vectors following the discretized dynamics:
$$ \underline{x_{i+1}} = \left[ \begin{array}{c} x_{i}+\Delta_{s_i} * \cos(\theta_i)\\ y_{i}+\Delta_{s_i} * \sin(\theta_i)\\ \theta_{i}+\Delta_{s_i} * \kappa_i \end{array} \right] $$
And I would like to make use of this discretized solution as an initial guess. The problem is that I would prefer to work in the continuous domain and once a solution is found I would discretized it (I have found solutions where I insert the discrete dynamics with my initial guess and I obtain a solution but do not want to include $\Delta_{s}$ as an optimization parameter because then I would be doing trajectory optimization instead of path optimization).
So as a summary:
- I want to obtain a continuous function that follows the continuous dynamics of my vehicle and where the initial point and final point are defined.
- I want this continuous function to minimize a certain cost function
- I would like to use a discretized solution that it has been provided to me as an initial guess (maybe interpolate the provided solution?)
Approaches I have done:
- iLQR and DDP to the discretized system using the original control input as first guess. The problem is that the control input needs to be augmented with $\Delta_{s_i}$ if we want to smooth a path composed of N points but we don't want to overshoot the target.
- Just consider a set of points and using gradient techniques optimized the cost function and do reverse engineering to find the corresponding control inputs to reach each point. Problem: The dynamics of the vehicle are not taken into consideration.
I would be thankful if somebody sheds some light into my problem as I am quite stuck right now.