6.3. Path Planning for a Simple Car

Keywords: optimal control, path planning, ipopt usage, rescaling time

6.3.1. Imports

The following cell checks if pyomo and ipopt are installed and, if not, executes commands to perform the necessary installation. This code is not bullet proof. If installation errors occur, try to install manually before proceeding further.

# install Pyomo and solvers
import requests
import types

url = "https://raw.githubusercontent.com/jckantor/ND-Pyomo-Cookbook/main/python/helper.py"
helper = types.ModuleType("helper")
exec(requests.get(url).content, helper.__dict__)

helper.install_pyomo()
helper.install_ipopt()

pyomo was previously installed
ipopt was previously installed

True

6.3.2. Mathematical model

The following equations describe a simple model of a car at low speeds

\[\begin{split} \begin{align} \frac{dx}{dt} & = v \cos(\theta) \\ \frac{dy}{dt} & = v \sin(\theta) \\ \frac{d\theta}{dt} & = \frac{v}{L}\tan(\phi) \\ \frac{dv}{dt} & = a_v \\ \end{align} \end{split}\]

where \(x\) and \(y\) denote the position of the center of the rear axle, \(v\) is velocity, \(\theta\) is the angle of the car axis to the horizontal, and \(\phi\) is the angle of the front steering wheels relative to the car axis. The car length \(L\) is the distance between the front and rear axles. The car responds to acceleration input \(a_v\) and and steering input \(\phi\).

The lateral acceleration (or lateral “g” force) on the car is \(v^2/R\) where \(R = L/\sin(\phi)\) is the radius of curvature. Denoting lateral acceleration as \(a_r\),

\[\begin{split} \begin{align} a_r & = \frac{v^2 \sin(\phi)}{L} \\ \end{align} \end{split}\]

The path planning problem is to find values \(a_v(t)\) and \(\phi(t)\) on an interval \(0 \leq t \leq t_f\) which steer the car from an initial condition \(\left[x(0), y(0), \theta(0), u(0)\right]\) to a specified final condition \(\left[x(t_f), y(t_f), \theta(t_f), u(t_f)\right]\). The desired control inputs minimizes an objective function:

\[ \begin{align} J = \min \int_0^{t_f} \left( a_v(t)^2 + \gamma a_r(t)^2 \right)\,dt \end{align} \]

subject to the following limits \(a_v\), \(\phi\), and \(v\).

variable	bound
$	a_r
$	a_v
$	\phi
\(v\)	30 \(m/s\)
\(-v\)	4 \(m/s\)

The next cells implement the model using a fixed estimate for \(t_f\), the time required to complete the desired maneuver.

6.3.3. Pyomo model

from pyomo.environ import *
from pyomo.dae import *

# parameters
ar_max = 2.8
av_max = 2.8
phi_max = 0.7
v_max  = 30
v_min = -4

# time and length scales
L = 5
tf = 40

# create a model object
m = ConcreteModel()

# define the independent variable
m.t = ContinuousSet(bounds=(0, tf))

# define control inputs
m.av = Var(m.t)
m.phi = Var(m.t, bounds=(-phi_max, phi_max))

# define the dependent variables
m.x = Var(m.t, domain=NonNegativeReals)
m.y = Var(m.t, domain=NonNegativeReals)
m.a = Var(m.t)
m.v = Var(m.t)

# define derivatives
m.x_dot = DerivativeVar(m.x)
m.y_dot = DerivativeVar(m.y)
m.a_dot = DerivativeVar(m.a)
m.v_dot = DerivativeVar(m.v)

# define the differential equation as constraints

@m.Constraint(m.t)
def ode_x(m, t):
    return m.x_dot[t] == m.v[t]*cos(m.a[t])

@m.Constraint(m.t)
def ode_y(m, t):
    return m.y_dot[t] == m.v[t]*sin(m.a[t])

@m.Constraint(m.t)
def ode_a(m, t):
    return m.a_dot[t] == m.v[t]*tan(m.phi[t])/L

@m.Constraint(m.t)
def ode_v(m, t):
    return m.v_dot[t] == m.av[t]

# path constraints
m.path_v1 = Constraint(m.t, rule=lambda m, t: m.v[t] <= v_max)
m.path_v2 = Constraint(m.t, rule=lambda m, t: m.v[t] >= v_min)
m.path_a1 = Constraint(m.t, rule=lambda m, t: m.av[t] <= av_max)
m.path_a2 = Constraint(m.t, rule=lambda m, t: m.av[t] >= -av_max)
m.path_a3 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t])/L <= ar_max)
m.path_a4 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t])/L >= -ar_max)

# initial conditions
m.pc = ConstraintList()
m.pc.add(m.x[0]==0)
m.pc.add(m.y[0]==0)
m.pc.add(m.a[0]==0)
m.pc.add(m.v[0]==0)

# final conditions
m.pc.add(m.x[tf]==0)
m.pc.add(m.y[tf]==4*L)
m.pc.add(m.a[tf]==0)
m.pc.add(m.v[tf]==0)

# final conditions on the control inputs
m.pc.add(m.av[tf]==0)
m.pc.add(m.phi[tf]==0)

# define the optimization objective
m.integral = Integral(m.t, wrt=m.t, rule=lambda m, t: m.av[t]**2 + (m.v[t]**2*sin(m.phi[t])/L)**2)
m.obj = Objective(expr=m.integral)

# transform and solve
TransformationFactory('dae.finite_difference').apply_to(m, wrt=m.t, nfe=30)
SolverFactory('ipopt').solve(m).write()

# ==========================================================
# = Solver Results                                         =
# ==========================================================
# ----------------------------------------------------------
#   Problem Information
# ----------------------------------------------------------
Problem: 
- Lower bound: -inf
  Upper bound: inf
  Number of objectives: 1
  Number of constraints: 440
  Number of variables: 310
  Sense: unknown
# ----------------------------------------------------------
#   Solver Information
# ----------------------------------------------------------
Solver: 
- Status: ok
  Message: Ipopt 3.13.4\x3a Optimal Solution Found
  Termination condition: optimal
  Id: 0
  Error rc: 0
  Time: 3.7143239974975586
# ----------------------------------------------------------
#   Solution Information
# ----------------------------------------------------------
Solution: 
- number of solutions: 0
  number of solutions displayed: 0

6.3.4. Accessing solution data

import numpy as np
import matplotlib.pyplot as plt

# access the results
t = np.array([t for t in m.t])

av = np.array([m.av[t]() for t in m.t])
ar = np.array([(m.v[t]()**2 * np.sin(m.phi[t]()))/L for t in m.t])
phi = np.array([m.phi[t]() for t in m.t])

x = np.array([m.x[t]() for t in m.t])
y = np.array([m.y[t]() for t in m.t])
a = np.array([m.a[t]() for t in m.t])
v = np.array([m.v[t]() for t in m.t])

def plot_results(t, x, y, a, v, av, phi):
    fig, ax = plt.subplots(3,1, figsize=(10,8))

    ax[0].plot(t, av, t, v**2*np.sin(phi)/L)
    ax[0].legend(['Acceleration','Lateral Acceleration'])

    ax[1].plot(t, phi, t, a)
    ax[1].legend(['Wheel Position','Car Direction'])

    ax[2].plot(t, v)
    ax[2].legend(['Velocity'])
    ax[2].set_ylabel('m/s')
    for axes in ax:
        axes.grid(True)
        
plot_results(t, x, y, a, v, av, phi)

6.3.5. Visualizing the car path

scl=0.3

def draw_car(x=0, y=0, a=0, phi=0):
    R = np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]])
    car = np.array([[0.2, 0.5], [-0.2, 0.5], [0, 0.5], [0, -0.5],
                    [0.2, -0.5], [-0.2, -0.5], [0, -0.5], [0, 0], [L, 0], [L, 0.5],
                    [L + 0.2*np.cos(phi), 0.5 + 0.2*np.sin(phi)],
                    [L - 0.2*np.cos(phi), 0.5 - 0.2*np.sin(phi)], [L, 0.5],[L, -0.5],
                    [L + 0.2*np.cos(phi), -0.5 + 0.2*np.sin(phi)],
                    [L - 0.2*np.cos(phi), -0.5 - 0.2*np.sin(phi)]])
    carz = scl*R.dot(car.T)
    plt.plot(x + carz[0], y + carz[1], 'k', lw=2)
    plt.plot(x, y, 'k.', ms=10)
    
plt.figure(figsize=(10,10))
for xs,ys,ts,ps in zip(x, y, a, phi):   
    draw_car(xs, ys, ts, scl*ps)
plt.plot(x, y, 'r--', lw=0.8)
plt.axis('square')
plt.grid(True)

6.3.6. Rescaling to incorporate time into the optimization model

While out initial formulation of the path planning problem leads to a solution, one unsatisfactory aspect of the formulation is the need to estimate the time necessary to complete the maneuver. Here we rescale the problem in such a way that the time horizon becomes part of the optimization calculation.

The following equations describe a simple model of a car

\[\begin{split} \begin{align} \frac{dx}{dt'} & = t_f v \cos(\theta) \\ \frac{dy}{dt'} & = t_f v \sin(\theta) \\ \frac{d\theta}{dt'} & = \frac{t_f }{L}\tan(\phi) \\ \frac{dv}{dt'} & = t_f a_v \\ \end{align} \end{split}\]

subject to bounds on the manipulable variables.

\[\begin{split} \begin{align} | a_v | & \leq a^{max}_v \\ |\frac{v^2\sin(\phi)}{L}| & \leq a^{max}_r \\ |\phi | & \leq \phi^{max} \\ | v | & \leq v^{max} \\ \end{align} \end{split}\]

The objective function

\[ \begin{align} J = t_f \min \int_0^{1} \left( \phi(t)^2 + \alpha a(t)^2 + \beta v(t)^2\right)\,dt' \end{align} \]

Introduce the natural scales

\[\begin{split} \begin{align} x' & = \frac{x}{L} \\ y' & = \frac{y}{L} \\ v' & = \frac{t_f}{L} v \\ a'_v & = \frac{t_f^2}{L} a_v\\ \end{align} \end{split}\]

Resulting

\[\begin{split} \begin{align} \frac{dx'}{dt'} & = v' \cos(\theta) \\ \frac{dy'}{dt'} & = v' \sin(\theta) \\ \frac{d\theta}{dt'} & = v' \tan(\phi) \\ \frac{dv'}{dt'} & = a'_v \\ \end{align} \end{split}\]

The bounds become

\[\begin{split} \begin{align} |a'_v| & \leq \frac{t_f^2}{L}a^{max}_v \\ |v'^2\sin(\phi)| & \leq \frac{t_f^2}{L}a^{max}_r \\ |\phi | & \leq \phi^{max} \\ | v' | & \leq \frac{t_f}{L}v^{max} \\ \end{align} \end{split}\]

In the rescaled variables, the objective function becomes

\[ \begin{align} J = \min \left[ t_f + \frac{L^2}{t_f^3} \int_0^1 \left(\gamma_v a_v^2 + \gamma_r v^4 \sin^2(\phi) \right)\,dt\right] \end{align} \]

where an additional term \(t_f\) has been included to incorporate a tradeoff between applied acceleration and corning forces and the time required to complete the maneuver.

# parameters
ar_max = 2.8
av_max = 2.8
phi_max = 0.7
v_max  = 30
v_min = -4

# time and length scales
L = 5

# create a model object
m = ConcreteModel()

# define the independent variable
m.tf = Var(domain=NonNegativeReals)
m.t = ContinuousSet(bounds=(0, 1))

# define control inputs
m.av = Var(m.t)
m.phi = Var(m.t, bounds=(-phi_max, phi_max))

# define the dependent variables
m.x = Var(m.t, domain=NonNegativeReals)
m.y = Var(m.t, domain=NonNegativeReals)
m.a = Var(m.t)
m.v = Var(m.t)

# define derivatives
m.x_dot = DerivativeVar(m.x)
m.y_dot = DerivativeVar(m.y)
m.a_dot = DerivativeVar(m.a)
m.v_dot = DerivativeVar(m.v)

# define the differential equation as constraints

@m.Constraint(m.t)
def ode_x(m, t):
    return m.x_dot[t] == m.v[t]*cos(m.a[t])

@m.Constraint(m.t)
def ode_y(m, t):
    return m.y_dot[t] == m.v[t]*sin(m.a[t])

@m.Constraint(m.t)
def ode_a(m, t):
    return m.a_dot[t] == m.v[t]*tan(m.phi[t])/L

@m.Constraint(m.t)
def ode_v(m, t):
    return m.v_dot[t] == m.av[t]

# path constraints
m.path_v1 = Constraint(m.t, rule=lambda m, t: m.v[t] <= m.tf*v_max/L)
m.path_v2 = Constraint(m.t, rule=lambda m, t: m.v[t] >= m.tf*v_min/L)
m.path_a1 = Constraint(m.t, rule=lambda m, t: m.av[t] <= m.tf**2*av_max/L)
m.path_a2 = Constraint(m.t, rule=lambda m, t: m.av[t] >= -m.tf**2*av_max/L)
m.path_a3 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t]) <= m.tf**2*ar_max/L)
m.path_a4 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t]) >= -m.tf**2*ar_max/L)

# initial conditions
m.x[0].fix(0)
m.y[0].fix(0)
m.a[0].fix(0)
m.v[0].fix(0)

# final conditions
m.x[1].fix(0)
m.y[1].fix(4)
m.a[1].fix(0)
m.v[1].fix(0)

# final conditions on the control inputs
m.av[1].fix(0)
m.phi[1].fix(0)

# define the optimization objective
m.integral = Integral(m.t, wrt=m.t, rule=lambda m, t: m.av[t]**2 + (m.v[t]**2*sin(m.phi[t]))**2)
m.obj = Objective(expr= m.tf + L**2*m.integral/m.tf**3)

# transform and solve
TransformationFactory('dae.finite_difference').apply_to(m, wrt=m.t, nfe=30)
SolverFactory('ipopt').solve(m).write()

# ==========================================================
# = Solver Results                                         =
# ==========================================================
# ----------------------------------------------------------
#   Problem Information
# ----------------------------------------------------------
Problem: 
- Lower bound: -inf
  Upper bound: inf
  Number of objectives: 1
  Number of constraints: 430
  Number of variables: 301
  Sense: unknown
# ----------------------------------------------------------
#   Solver Information
# ----------------------------------------------------------
Solver: 
- Status: ok
  Message: Ipopt 3.13.4\x3a Optimal Solution Found
  Termination condition: optimal
  Id: 0
  Error rc: 0
  Time: 1.3755340576171875
# ----------------------------------------------------------
#   Solution Information
# ----------------------------------------------------------
Solution: 
- number of solutions: 0
  number of solutions displayed: 0

# access the results
t = np.array([t*m.tf() for t in m.t])

av = np.array([m.av[t]()*L/(m.tf()**2) for t in m.t])
phi = np.array([m.phi[t]() for t in m.t])

x = np.array([m.x[t]()*L for t in m.t])
y = np.array([m.y[t]()*L for t in m.t])
a = np.array([m.a[t]() for t in m.t])
v = np.array([m.v[t]()*L/m.tf() for t in m.t])

ar = v**2 * np.sin(phi)/L
        
plot_results(t, x, y, a, v, av, phi)

scl=0.2

plt.figure(figsize=(10,10))
for xs,ys,ts,ps in zip(x, y, a, phi):   
    draw_car(xs, ys, ts, scl*ps)
plt.plot(x, y, 'r--', lw=0.8)
plt.axis('square')
plt.grid(True)

print(m.tf())

37.57248949045362