motion model for forward movement

Question

I thought this is a simple and well known problem and therefore there is a lot of literature. But I'm not able to find the correct keywords to google the answer. Maybe instead of answering the whole question you can already help me by telling me the correct keywords.

My problem is the following:

For a given parametrisation of a robot, for example the simple two dimensional $(\varphi,p_1,p_2)$, where $\varphi$ describes the orientation and $p$ the 2D position, I want to find a trajectory

$c:[0,1]\rightarrow S^1\times R^2$

from one given pose $(\varphi_0,(p_1)_0,(p_2)_0)$ to another given pose $(\varphi_1,(p_1)_1,(p_2)_1)$. This trajectory should also be optimal with respect to an energy functional for example

$\int_0^1 \varphi^2+p^2_1+p^2_2 \;dt$

and I want two constraints to be fulfilled in order to make sure that the robot moves only forward. These constraints are:

$\dot{p_1}=\cos(\varphi)$

$\dot{p_2}=\sin(\varphi)$

With Euler-Langrange I get the ODE system:

$\ddot{\varphi}=\varphi^2$

$\ddot{p_1}=-sin(\varphi)\dot{p_1}$

$\ddot{p_2}=cos(\varphi)\dot{p_2}$

Now I'm wondering if this equation system is solvable with my boundary conditions.

My questions are 1) Are there any theorems that I can use to make statements about the existence of solutions? Whenever I tried to solve this ODE in Matlab the second boundary condition was not reached.

2) Would it be easier to proof the existence if I would use another model for example with an orientation independent stearing wheel?

3) How can I solve this equation system?

4) In literature I only found very complicated models with a lot more parameters or the paper was not about reaching a specific goal pose can you advise me more basic literature with approches similar to what I derived here?

One step back to the onedimensional model

My parameters are $(p,v)\in R^2$, where $p$ indicates the onedimensional position and $v$ the velocity.

I used the energyfuctional $\int v^2 dt$ and the constraint $\dot{p}=v$. The resulting ODE system is

$\ddot{x}_1=\dot{v}$

$\ddot{x}_2=0$

I didn't find a solution for the boundary values $(0,1)$ and $(1,1)$.

And a new question araised. Maybe I should not assume that I get a continuous solustion for all my model parameters?

Answer 1

Let me start by saying I haven't done trajectory optimization or anything, but I've done quite a bit of differential equations for controls. I think your equations might not be solvable because of the way you've defined your constraint conditions:

$$ \dot{p}_1=\cos{\phi} \\ \dot{p}_2=\sin{\phi} \\ $$

If you want your heading/orientation to remain steady, how do you change the translational speeds? In your ODE system,

$$ \ddot{\phi} = \phi^2 $$

What if your starting heading $\phi(0) = 0$? It would look like $\ddot{\phi}$ is "trapped" to be always zero, and your translational accelerations are always stuck at $\ddot{p}_1=−\sin(0)\dot{p}_1 = 0$ and $\ddot{p}_2=\cos(0)\dot{p}_2 = \dot{p}_2$.

Given your constraint conditions $\dot{p_1} = \cos(0) = 1$ and $\dot{p_2} = \sin(0) = 0$. The constraints and accelerations seem to match, but it's a trivial solution that doesn't get you anywhere; $\dot{p}_1$ is a constant speed of $1$ and $\dot{p}_2$ is a constant speed of $0$, so again it looks like you're not able to change speed (e.g., stop) and you're not able to change position at all on the $p_2$ axis.

You haven't provided a diagram of your system, so I'm not sure why you have that "forward motion" is defined by the sine/cosine of the heading. If I had to guess, I would think maybe you're looking at a steered vehicle, but in that instance you'd have an extra term - the vehicle's linear speed - and this would be reflected in your constraints:

$$ \dot{p}_1 = \dot{x}\left(\cos{\phi}\right) \\ \dot{p}_2 = \dot{x}\left(\sin{\phi}\right) \\ $$

This might be what's missing from your equations that's precluding you from obtaining a solution. Again, I haven't done anything with trajectory optimization, but in looking around I found the following paper that might be of use:

Guidelines in Nonholonomic Motion Planning for Mobile Robots

J.P. Laumond S. Sekhavat F. Lamiraux

This is the first chapter of the book: Robot Motion Planning and Control

Jean-Paul Laumond (Editor)

You had asked too about terms to search, and I found this paper by looking for "nonholonomic trajectory control."

Answer 2

with the key words chuck gave me I found an article of David Anisi about "Optimal motion control of a ground vehicle".

There he describes an algorithm using the Hamilton to solve these kind of optimal control problems. Chuck was right that I had to add parameters for linear as well as angular velocity. Therefore my constraints are

$\dot{p_1}=v cos(\phi)$

$\dot{p_2}=v sin(\phi)$

$\dot{p_3}=w$

The linear velocity $v$ and the angular velocity $w$ are not part of the parametrisation but control variables. The function $L$ simplifies to $L=v^2+w^2$. The right hand sides of the constraints are abreviated with $f$.

The Algorithm is:

1) define $H(x,v,w,\lambda):=-L+\lambda f$

2) find the maximal argument $\bar{v}$ $\bar{w}$ s.t. H is maximal.

3) the derivations of $H(x,\bar{v},\bar{w},\lambda)$ with respect to $\lambda$ define the derivatives of $x$ and the negative derivations of $H(x,\bar{v},\bar{w},\lambda)$ with respect to $x$ define the derivatives of $\lambda$.

4) this ode system can be solved with the shooting method