PROPT Obstacle Avoidance

From TomWiki
Revision as of 08:10, 9 November 2011 by Mbot (talk | contribs)
Jump to navigationJump to search

Notice.png

This page is part of the PROPT Manual. See PROPT Manual.

OPTRAGEN 1.0 A MATLAB Toolbox for Optimal Trajectory Generation, Raktim Bhattacharya, Texas A&M University (Note: There is typographical error in the OPTRAGEN documentation. The objective involves second derivatives of x and y.)

A robot with obstacles in 2D space. Travel from point A to B using minimum energy.

Problem Formulation

Find theta(t) and V over t in [0; 1 ] to minimize


subject to:


Where V is a constant scalar speed.

% Copyright (c) 2007-2008 by Tomlab Optimization Inc.

clear pi % 3.14159... unless someone has set it to something else!

toms t V

t_f = 1; % final time

% Starting guess
speed = 5;
xopt = 1.2*t;
yopt = 1.6*t;
vxopt = 1.2;
vyopt = 1.6;
thetaopt = pi/4;

Solve the problem, using a successively larger number collocation points

for n=[4 15 30]


    % Create a new phase and states, using n collocation points
    p = tomPhase('p', t, 0, t_f, n);
    setPhase(p);
    tomStates x y vx vy
    tomControls theta

    % Interpolate an initial guess for the n collocation points
    x0 = {V == speed
        icollocate({x == xopt; y == yopt; vx == vxopt; vy == vyopt})
        collocate(theta == thetaopt)};

    % Box constraints
    cbox = {0 <= V <= 100 };

    % Boundary constraints
    cbnd = {initial({x == 0; y == 0})
        final({x == 1.2; y == 1.6})};

    % ODEs and path constraints
    ode = collocate({
        dot(x) == vx == V*cos(theta)
        dot(y) == vy == V*sin(theta)
        });

    % A 30th order polynomial is more than sufficient to give good
    % accuracy. However, that means that mcollocate would only check
    % about 60 points. In order to make sure we don't hit an obstacle,
    % we check 300 evenly spaced points instead, using atPoints.
    obstacles = atPoints(linspace(0,t_f,300), {
        (x-0.4)^2 + (y-0.5)^2 >= 0.1
        (x-0.8)^2 + (y-1.5)^2 >= 0.1});

    % Objective: minimum energy.
    objective = integrate(dot(vx)^2+dot(vy)^2);

Solve the problem

    options = struct;
    options.name = 'Obstacle avoidance';
    constr = {cbox, cbnd, ode, obstacles};
    solution = ezsolve(objective, constr, x0, options);

    % Optimal x, y, and speed, to use as starting guess in the next iteration
    xopt = subs(x, solution);
    yopt = subs(y, solution);
    vxopt = subs(vx, solution);
    vyopt = subs(vy, solution);
    thetaopt = subs(theta, solution);
    speed = subs(V,solution);
Problem type appears to be: qpcon
Time for symbolic processing: 0.16968 seconds
Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - TOMLAB Development license  999007. Valid to 2011-12-31
=====================================================================================
Problem: ---  1: Obstacle avoidance             f_k      29.812856165009947000
                                       sum(|constr|)      0.000000001309307815
                              f(x_k) + sum(|constr|)     29.812856166319254000
                                              f(x_0)      0.000000000000062528

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv   22 ConJacEv   22 Iter   20 MinorIter 2732
CPU time: 0.218401 sec. Elapsed time: 0.218000 sec. 

Problem type appears to be: qpcon
Time for symbolic processing: 0.17036 seconds
Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - TOMLAB Development license  999007. Valid to 2011-12-31
=====================================================================================
Problem: ---  1: Obstacle avoidance             f_k      22.128728366252528000
                                       sum(|constr|)      0.000000000006755102
                              f(x_k) + sum(|constr|)     22.128728366259281000
                                              f(x_0)     29.812856165012775000

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv  151 ConJacEv  151 Iter  136 MinorIter  486
CPU time: 0.920406 sec. Elapsed time: 0.915000 sec. 

Problem type appears to be: qpcon
Time for symbolic processing: 0.16882 seconds
Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - TOMLAB Development license  999007. Valid to 2011-12-31
=====================================================================================
Problem: ---  1: Obstacle avoidance             f_k      22.091923284169869000
                                       sum(|constr|)      0.000000000011094717
                              f(x_k) + sum(|constr|)     22.091923284180965000
                                              f(x_0)     22.128728366262933000

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv  309 ConJacEv  309 Iter  281 MinorIter  720
CPU time: 4.071626 sec. Elapsed time: 3.770000 sec. 


end

Plot result

figure(1)
th = linspace(0,2*pi,500);
x1 = sqrt(0.1)*cos(th)+0.4;
y1 = sqrt(0.1)*sin(th)+0.5;
x2 = sqrt(0.1)*cos(th)+0.8;
y2 = sqrt(0.1)*sin(th)+1.5;
ezplot(x,y);
hold on
plot(x1,y1,'r',x2,y2,'r');
hold off
xlabel('x');
ylabel('y');
title(sprintf('Obstacle avoidance state variables, Speed = %2.4g',speed));
axis image

ObstacleAvoidance 01.png