Optimal trajectory for manipulators using optimal control

Question

I'm trying to implement direct-multiple shooting method to my problem.

Objective function: tf
constraints       : q<q_max
                    v<v_max (v=dq/dt)
                    a<a_max
                    tau<tau_max (tau=M(q)a+B(q,v)+G(q))
                    C(q)=r_0-|P-P_0|  (obstacle avoidance)

Initial condition q(0)=q_0 (q_0 is given) 
                  q(t_f)=q_f (q_f is given) and 
                  v(0)= 0
                  v(t_f)=0

As I understand from the theory, I have to divide the variables as state variables and control variables.

State variables are: q and v 
Control variable is: tau
In each time interval I'll generate cubic splines which are q(t)=a_0+a_1*t+a_2*t^2+a_3*t^3

Could you help me how I will implement it? I don't understand what is the ODE here and how I should construct the algorithm?

Are there any example about it?

edit to make the equations clear I'll rewrite them here again:

based on the link

state variables: x1(t) = (q1(t) , ··· ,qn(t))^T and x2(t) = (q˙1(t) , ··· ,q˙n(t))^T. and derivatives of the state variables are equal to x˙(t) = f(x(t) ,u(t)) where f is f(x(t), u(t)) = ((q˙1(t), . . . , q˙5(t))^T; M(x(t))−1· (u(t) − N(x(t)))

I don't know how to insert cubic polynomials in that equation system and how to solve ODE Will it be like [T,X]=ode45('f', [0 t_f], [q_0 q_f])

Answer 1

You have asked a complex question, and from your question it seems that there a lot of basic underlying concepts that you are not familiar with. Since you are new to ODEs, path planning and possibly robotics I would advise to take a stepwise approach:

1. Take a look at how s-curves trajectory planning is done for robotic manipulators. You will se how the trajectory equation looks like for positions, velocities, accelerations and possibly jerks. The way the trajectory is defined will surly explain what is the ODE when talking about trajectory planning. You can find explanations here and here here and second half of this document

2. Take a look at path planning with obstacle avoidance. Here you can learn how optimality criteria (cost function) are defined and used. Here and here and the first half of this document

3. Read about optimal control. Before solving complex problems with optimal control, solve simple examples and make sure you understand them. Introduction here

4. Combine all of the above. If you know how trajectory planning works, how path planning works and how optimal control works, you are in a position to combine them.

Answer 2

@chuck: thanks for reading my answer and give advice that there is room for improvement. I would like to try out the edit button to improve my answer.

But to the topic:

A lock into history of optimal control may help to answer the question. A classical problem which is over 200 years old is the two body problem. Later in 1760 Leonhard Euler described the three-body-problem. In both cases, a mathematical system is used for describing the problem itself. The formula which describes the three body problem and the problem from the original post also are called "ordinary differential equation" (ODE). The solution for solving these kind of problem is to use a transfer function which is a backward ODE.

In robotics for most problems such a backward function is impossible to create because the mathematical theory has no answer. So in most cases the three-body-problem and also robotics problems are solved with numerical solver which are not very elegant but uses bruteforce searching to detemine the parameter.

Could you help me how I will implement it?

Optimal Control with obstacle avoidance can only be done with a numerical solver like A* or others. It is not poassible to use a ODE for this kind of task, it is a np-hard problem. An example implementation in matlab is done by. The author used the "OPTRAGEN 1.0 A MATLAB Toolbox for Optimal Trajectory Generation" for generating trajectory between obstacles.