Please I am looking for ideas on how to solve a problem.
I have an autonomous vehicle that can move on its own. I am trying to set up an optimization algorithm with a cost function that will ensure that the vehicle does not leave a particular geographical area or it should go back to the predefined area if it leaves. For example, If I deploy the vehicle in London, It should remain in London.
I wanted a convex, continuous, and twice differentiable cost function to guarantee the stability requirement of my algorithm.
So far, I have been trying to minimize the distance between the vehicle's current position and the desired position. But it is difficult to achieve. My cost function is shown below.
$$ y = (x_{latitude} - D_{latitude})^2 + (x_{longitude} - D_{longitude})^2 $$
$x_{latitude}$ and $x_{longitude}$ are the current position vehicle current position.
This cost function, will steer the vehicle to a point: namely $D_{longitude}$ and $D_{latitude}$. This requirement is too restrictive.
Any ideas will be appreciated.
Instead of a cost function, why not impose constraints? For example, you can enforce that the vehicle stay within a rectangular region by imposing bounds $$\ell_\text{latitude} \le x_\text{latitude} \le u_\text{latitude} \\ \ell_\text{longitude} \le x_\text{longitude} \le u_\text{longitude}$$ More generally, you can impose linear constraints to restrict the vehicle to a polygonal region.