Global Trajectory Optimization

After importing, create a new global opt object:

g = GlobalOptCompiled(N)
Copy

where N is the number of points to expect in the discretized path.

You can also pass some other options. Here’s the full constructor:

GlobalOptCompiled(N, 
                  nlp_solver='ipopt', # solver library. use 'worhp' if you can
                  solver_opts=None,   # options for solver (defaults are good)
                  car_params={
                      'l_r': 1.4987,  # length from CG to front axle (m)
                      'l_f': 1.5213,  # length from CG to back axle (m)
                      'm'  : 1.0,     # mass (1.0 => force=acc; don't change)
                  },
                  bbox = {
                      'l': 2.7+0.5,   # length of car bounding box plus tolerance
                      'w': 1.6+0.5,   # width of car bounding box plus tolerance
                  },
                  DF_MIN  =      - 0.5, # min steering angle
                  DF_MAX  =        0.5, # max steering angle
                  ACC_MIN =      - 3.0, # max braking acceleration (negative) 
                  ACC_MAX =        2.0, # max forward acceleration
                  ACC_MAX_FN = ACC_MAX  # also limit fwd acc < ACC_MAX_FN(velocity)
                  DF_DOT_MIN =   - 0.5, # max steering derivative (left) (rad/s)
                  DF_DOT_MAX =     0.5, # max steering derivative (right) (rad/s)
                  V_MIN =          0.0, # minimum forward velocity
                  V_MAX =         25.0, # maximum forward velocity
                  FRIC_MAX =      12.0) # maximum acceleration (based on tire grip)
Copy

FRIC_MAX and ACC_MAX should be of type casadi.Function with the signature (i0)->(o0) (ie, one input and one output). They are functions of velocity, in m/s. FRIC_MAX can also be passed as a float, corresponding simply to a constant function.

To use the GlobalOptSettings class, do this:

from global_opt_settings import GlobalOptSettings
g = GlobalOptCompiled(**GlobalOptSettings)
Copy

To run the solver, use g.solve:

g.solve(left, right)
Copy

where left and right are the left and right sides of the track. They should be of shape (N, 2), where N is the same as the one you passed in to the constructor. These points determine the discretization of the track - the optimizer simply linearly interpolates between corresponding pairs of points. Choosing these points given a path of cones is a good deal of effort. Yash's cone ordering algorithm will be the final answer to this problem, but for now I've implemented an okay solution with the PairPath class in MPC/custom_opt/pairpath.py.

PairPath uses the vertices of a Delaunay triangulation to find the center, then uses that to order and evenly distribute points.

The solve will return a dictionary with three items:

• z is the full state of the car. It has shape (N, 4), and its format is .
• u is the control inputs. It has shape (N, 2), and its format is .
• t is the timestamps corresponding to each element of z and u. It will always be an increasing series, but it is not evenly spaced.

data with inconsistent timesteps is really quite difficult to deal with, so we need to fix that. Use the g.to_constant_tgrid method:

res = g.solve(left, right)
states, controls = g.to_constant_tgrid(dt=0.1, **res)
Copy

(enter whatever timestep you like for dt)

This returns two numpy arrays, here states and controls. states has shape (M, 4), and controls has shape (M, 2). These states and controls represent a real, feasible (to within the error of the integrator) path with even time steps of dt seconds.

We’re given the track in pairs of points defining the boundaries. We use this to create a parameterized path through the track in coordinates, then add the rest of the state and controls at each point. The time between points - - is also a variable.

Here’s an illustration:

we’re given and . We set up the optimization problem:

Now let’s define all those variables and functions!

Let our to be all of our variables together (please excuse this abuse of notation):

Our cost function is simply the lap time:

Now we have to make our constraint functions. First, we’ll introduce the vehicle dynamics. Let represent a function which integrates the dynamics. Then the constraints are:

where and .

This function is implemented using the midpoint method in discrete_custom_integrator from MPC/sim/sim.py.

Now we also want to add constraints based on the car’s limits:

• The car can only accelerate so fast, and that acceleration depends on the current motor speed.
  • for some function determined empirically using data from dynamics:
    
• The car can only brake so hard.
  
• The steering angle is mechanically limited
  
• the steering wheel can only be turned so fast
  
• the tire friction is limited, so centripetal and driving acceleration must not exceed friction limits. These friction limits depend on velocity, since aero increases downforce.
  
  this is an example plot from the OpenLapSim documentation. We need to gather true data from dynanics.
  

These are all implemented fairly simply, using direct constraints on the input variables. The last one is a bit silly:

centripetal acceleration is given by: . is an optimization variable that we use directly, but is nontrivial to compute. It is a function of steering angle and velocity. We can take it directly from the dynamics:

which is where the expression for centripetal acceleration comes from.

ac = (self.v**2 / self.car_params['l_r']) * sin(arctan(self.car_params['l_r']/(self.car_params['l_f'] + self.car_params['l_r']) * tan(self.u[:, 1])))
Copy

Then, we can combine centripetal and forward acceleration with the Pythagorean theorem, since we know they’re orthogonal. This net acceleration can then be constrained to be less than some function of velocity.

it turns out our car isn't just a point - it's a whole car! This means we have to make sure we don't run into the cones.
The naive solution is just to bound to a smaller interval than , but it turns out that this doesn't work because the car has length as well as width. If we're going around a corner, we might hit cones with the front or back of the car, even if the sides are ok:

The fix for this issue is very simple. We simply add a constraint saying that none of the points defining the track may be within a certain bounding box of the car.

First, we transform the points to the reference frame of the car by getting the vector from the car to the track points then rotating by the negative of the car's heading.

The additional offset at the end is to correct for the difference between the car's center of mass and its centroid.

Once we have that, we just need to make sure and .

Unfortunately, I found that a strict rectangle like this makes the car confused. If it runs head on into a point, it doesn't know that it has to turn to get around it. Mathematically, this corresponds to the constraint having very limited information encoded in its derivative.

To fix this, the actual constraint is an approximation of a rectangle.

When plotted, we get this:

The exponent of 6 was chosen empirically to give a very rectangular shape while still encoding some roundness in the derivative and keeping numbers low enough to prevent numerical instability.

This worked much better. To get runtime down even more, we also limit the cones we consider to those 5 indices before and after the current index. This is possible since our progress along the track is entirely determined by our index in the track boundary pairs, unlike MPC where it's very hard to quantify ahead of time which points are nearest.

This reduces the number of points we must compute from to , which can be a massive improvement for high .

It’s all implemented using the base casadi.nlpsol class. This is used rather than the Opti stack because it’s more fully featured, we get more control, and most importantly, it has far better support for code generation, which will significantly reduce init and solve times.