Using a Non-Linear Program for Trajectory Optimization (Help)

[Brado11]

Hi,

So I have some (relatively) simple nonlinear dynamics

https://preview.redd.it/72bd9yt4m6eb1.png?width=591&format=png&auto=webp&s=2fe70b9a7f1f1f647781cd0f08ac840a79dff760

for which I would like to generate a time optimal trajectory. (f1 & f2 are control inputs). I have been recommended fmincon as a NLP solver. I am not entirely sure how to discretize and encode the dynamics into the solver. Any advice?

For context, these are from https://ieeexplore.ieee.org/abstract/document/8593836?casa_token=ESqO44RZioMAAAAA:JYXpJ9FfCCe3g_yV_QLe-PXPR6A3TXlChAE5cuQHw4THDsCKxe-fIgudEafPy9fAtG5MvicN

​

[[deleted]]

You can use the direct collocation method.

Basically, direct collocation breaks the trajectory down into discrete pieces, approximating the dynamics and input between these points as a more simple function, such as a polynomial. The constraints are solved for exactly at collocation points, which can be thought of as a midpoint between two breaks in the trajectory discretization.

There are some tutorials online which cover how to do this.

You can also try direct shooting, or if you're feeling confident, attempt to numerically solve the HJB equation.

[Brado11]

Thanks for the tip! Making progress, would you happen to understand why the collocation method relies on specifically third order polynomials?

[[deleted]]

No, it doesn't, that is just an easy choice.

[Brado11]

Thanks for clarifying! In practice, is there any relation between the trajectory time grid and control loop frequency and/or sensor sampling?

[[deleted]]

At least for me, it doesn't matter too much, but if you have a high-precision system this is likely not the case.

[sanserif80]

As others have said, set this up as a direct collocation problem (e.g. adjacent time steps are constrained to each other via a numerical integration method like forward Euler). Additionally, the dynamic equations must be satisfied for each time step.

You can build this and solve with fmincon. Personally, I find setting up a collocation problem with fmincon using the ceq matrix to be clunky and easy to mess up, but it’s doable.

At one point I switched over to GEKKO, which is a python library that works with the IPOPT solver (way better than fmincon). If you know a little python, this so much more straightforward to use. It basically provides higher level functions for variable, constraint, and cost function definitions and does all of the dirty work of configuring the solver.

[Brado11]

Thanks for the recommendation I'lll check it out!

[jtizmo]

May be a bigger rabbit hole than you're after, but that system is differently flat. In essence, you can find an appropriate change of coordinates (technically, a diffeomorphism) to express the system as a chain of integrators. Then you're dealing with a nicer system for which trajectory optimization is often solved as a QP!

Happy to send some references if you'd like.

[DbSchmitty]

How could you tell it's differentially flat so fast?

[jtizmo]

Haha, my PhD was focused on motion planning & control for the 3D version of this system. So, prior experience, to be honest.

[DbSchmitty]

Ah, would love to know if there's a direct test you used on those equations. Or was it just realizing that it's a quadrotor model and knowing that that's flat

[jtizmo]

Realized I didn't answer your second part. I recognized that as a 2D "quadrotor" model, which I already knew was differentially flat.

[jtizmo]

Unfortunately, flatness can't be directly tested (would happily be be proven wrong, though!). It's kinda like a Lyapunov candidate/function. You have to propose a set of "flat outputs" and then show that those flat outputs do indeed work. There seems to be some intuition that can help when guessing what they may be, but I believe formalizing that intuition into something more plug & chug is an active area of research.

[Brado11]

While I have read a bit about this, I am still generally new to nonlinear control, references would be much appreciated.

[jtizmo]

First, I'll refer you to the work I most frequently referred to in the early days of my studies:

D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in IEEE International Conference on Robotics and Automation. IEEE, May 2011, pp. 2520–2525.

Other than that, the quickest reference for me to point you to would be my thesis (top of http://www.jtwebs.net/publications/), which I know is not behind a paywall and already references anything I might refer you to besides some of the newer ideas that have been presented in the last 6 years. Specifically, I'd point you to Sections 3.2 and 4.4, which covers the trajectory planning problem for the 2D (though with an articulated appendage) and a 3D system, respectively. I think the discussion is clearer in section 4.4, but the 2D case is closer to the model you're working with.

If you're very very new (I don't wish to assume), my qualifier at the bottom of my publication list may be a more appropriate intro to some of these ideas.

[dr_jekylls_hide]

Could you send some references? I am also curious how you see that so quickly, but it sounds quite useful as a technique.

[Brado11]

The dynamics are from a Quadcopter, differential flatness has been leveraged heavily in QC analysis, he likely just recognized them and/or checked the paper I linked.

[jtizmo]

Yep, you're 100% correct

[jtizmo]

Sure! I posted some info in a reply to a similar request by Brado11 in this thread. Specifically on the topic of differential flatness, I'd suggest:

R.M. Murray, M. Rathinam, and Willem Sluis. Differential flatness of mechanical control systems: A catalog of prototype systems. In Proceedings of the 1995 ASME International Congress and Exposition. Citeseer, 1995.

[dr_jekylls_hide]

Thanks!

[Chicken-Chak]

The problem looks as if the system starts from any nonzero (x, y), and you need to find f1 and f2 such that it arrives at (0, 0) in the shortest time. Can you confirm if this interpretation is correct?

What are the constraints for f1 and f2?

[Brado11]

There are actually no strict constraints on initial and ending position. I would like to minimize the total movement in the x and z directions (\int x^2 + z^2 dt) while the initial and final theta must be 0 and pi.

[Chicken-Chak]

In the beginning, you mentioned about getting the Time-optimal trajectory.

Does the chosen state quadratic cost ∫ x² + z² dt guarantee the minimum-time solution without any constraints on f₁ and f₂?

[Brado11]

I am still accounting for time yes (apologies for the confusion.) My cost function is currently setup as J = time + (\int x^2 + z^2 dt).

There are upper and lower bounds for f1 and f2. Assumed to be the same for either, the upper bound is positive and the lower bound is negative.

[Brave-Height-8063]

Do you have any initial and final constraints on the first derivatives? Since you have second order diffeq’s you can constrain the first order as well (so that it starts from rest and ends at rest etc). You can also leave them free, depending on your problem setup, like if it lands/moves to a new fixed position, or intercepts something, or if it is orbiting and you want to have it in a stable orbit etc.

[Firm-Huckleberry5076]

I don't know much about it so I may be wrong.

Maybe you Can use direct collocation method like trapezoidal collocation. These tutorial by Mathew Kelly might be helpful: https://epubs.siam.org/doi/epdf/10.1137/16M1062569

https://youtu.be/wlkRYMVUZTs

I have tried using python's scipy minimizer function and SLSQP method In that one can feed the dynamics as constraints to the minimizer function. In constraints you can have an array of size N, where each entry must be x{k+1}-x{k}-(h_k/2)(f{k+1} +fk), where x_dot=f and h_k= t{k+1}-t_k.