# motion model for forward movement

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?

• Have you been able to do this with an easier problem? Starting with 1d problem may bring some insight. – holmeski Apr 4 at 15:46
• How would you do this onedimensional? Isn't it clear in this case that the trajectory would be a straight line, since there are no other trajectorys in one dimension? – Mathsfreak Apr 4 at 17:06
• I was thinking you could try it with position and velocity along a single dimension. The results to that problem might elucidate the complexities of your full nonlinear problem. – holmeski Apr 4 at 17:12
• I will add in the question above my onedimensional model, it also does not work yet... – Mathsfreak Apr 6 at 10:53

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} \\$$

$$\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."

• For anyone else coming to this from the future (almost peak coronavirus situation here now, April 6th, 2020), the following was also noted on the paper I linked: Previoulsy published as: Lectures Notes in Control and Information Sciences 229. Springer, ISBN 3-540-76219-1, 1998, 343p. – Chuck Apr 6 at 12:39
• thanks for your answer, that helped me a lot in the last weeks. I have one question left, what do you mean with diagram of the system. I there a commonly used method to describe problems like that one? – Mathsfreak Apr 23 at 8:48
• @Mathsfreak - I think the crux of your problem is/was that your variables may not have been defined appropriately; this is what I was saying when I mentioned that your constraints may be what is/was preventing you from getting a solution. A diagram, with axes, etc. labeled, would help to determine if your equations have been generated correctly. Regarding a "commonly used method," it's almost always the same - start with a free-body diagram, label your axes and forces, and generate the equations of motion from that. But again, it all starts with a well-labeled diagram! – Chuck Apr 23 at 18:29

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

• Hey, glad you got an answer! Thanks for following up and posting what you found out here. – Chuck Apr 26 at 16:46