{
"cells": [
{
"cell_type": "markdown",
"id": "edb7e2c6",
"metadata": {},
"source": [
"## Optimal Control\n",
"\n",
"Richard M. Murray, 31 Dec 2021 (updated 16 Jan 2022)\n",
"\n",
"This notebook contains an example of using optimal control for a vehicle steering system."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "7066eb69",
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"import control as ct\n",
"import control.optimal as opt\n",
"import time"
]
},
{
"cell_type": "markdown",
"id": "4afb09dd",
"metadata": {},
"source": [
"## Vehicle steering dynamics\n",
"\n",
"The vehicle dynamics are given by a simple bicycle model. We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal. The vehicle input is given by $(v, phi)$ where $v$ is the forward velocity of the vehicle and phi is the angle of the steering wheel. The model includes saturation of the vehicle steering angle."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "a6143a8a",
"metadata": {},
"outputs": [],
"source": [
"# Vehicle steering dynamics\n",
"#\n",
"# System state: x, y, theta\n",
"# System input: v, phi\n",
"# System output: x, y\n",
"# System parameters: wheelbase, maxsteer\n",
"#\n",
"from vehicle import vehicle, plot_lanechange"
]
},
{
"cell_type": "markdown",
"id": "64bd3c3b",
"metadata": {},
"source": [
"## Optimal trajectory generation\n",
"\n",
"We consider the problem of changing from one lane to another over a perod of 10 seconds while driving at a forward speed of 10 m/s."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "42dcbd79",
"metadata": {},
"outputs": [],
"source": [
"# Initial and final conditions\n",
"x0 = [0., -2., 0.]; u0 = [10., 0.]\n",
"xf = [100., 2., 0.]; uf = [10., 0.]\n",
"Tf = 10"
]
},
{
"cell_type": "markdown",
"id": "4e75a2c4",
"metadata": {},
"source": [
"### Approach 1: standard quadratic cost\n",
"\n",
"We can set up the optimal control problem as trying to minimize the distance form the desired final point while at the same time as not exerting too much control effort to achieve our goal.\n",
"\n",
"(The optimization module solves optimal control problems by choosing the values of the input at each point in the time horizon to try to minimize the cost. This means that each input generates a parameter value at each point in the time horizon, so the more refined your time horizon, the more parameters the optimizer has to earch over.)"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "984c2f0b",
"metadata": {},
"outputs": [],
"source": [
"# Set up the cost functions\n",
"Qx = np.diag([.1, 10, .1]) # keep lateral error low\n",
"Qu = np.diag([.1, 1]) # minimize applied inputs\n",
"quad_cost = opt.quadratic_cost(vehicle, Qx, Qu, x0=xf, u0=uf)\n",
"\n",
"# Define the time horizon (and spacing) for the optimization\n",
"# horizon = np.linspace(0, Tf, 5, endpoint=True)\n",
"horizon = np.linspace(0, Tf, 10, endpoint=True)\n",
"# horizon = np.linspace(0, Tf, 20, endpoint=True)\n",
"\n",
"# Provide an intial guess (will be extended to entire horizon)\n",
"bend_left = [10, 0.01] # slight left veer\n",
"\n",
"# Compute the optimal control, setting step size for gradient calculation (eps)\n",
"start_time = time.process_time()\n",
"result1 = opt.solve_ocp(\n",
" vehicle, horizon, x0, quad_cost, \n",
" initial_guess= bend_left,\n",
"# initial_guess=u2,\n",
" minimize_method='trust-constr',\n",
" minimize_options={'finite_diff_rel_step': 0.01},\n",
")\n",
"print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n",
"\n",
"# Extract and plot the results (+ state trajectory)\n",
"t1, u1 = result1.time, result1.inputs\n",
"t1, y1 = ct.input_output_response(vehicle, horizon, u1, x0)\n",
"plot_lanechange(t1, y1, u1, yf=xf[0:2])\n",
"print(\"Final state: \", y1[:,-1])"
]
},
{
"cell_type": "markdown",
"id": "b7cade52",
"metadata": {},
"source": [
"Note the amount of time required to solve the problem and also any warning messages about to being able to solve the optimization. You can try to adjust a number of factors to try to get a better solution:\n",
"* Try changing the number of points in the time horizon\n",
"* Try changing the optimization method (see commented out code)"
]
},
{
"cell_type": "markdown",
"id": "6a9f9d9b",
"metadata": {},
"source": [
"### Approach 2: input cost, input constraints, terminal cost\n",
"\n",
"The previous solution integrates the position error for the entire horizon, and so the car changes lanes very quickly (at the cost of larger inputs). Instead, we can penalize the final state and impose a higher cost on the inputs, resuling in a more gradual lane change.\n",
"\n",
"We also try using a different solver for this example. You can pass the solver using the `minimize_method` keyword and send options to the solver using the `minimize_options` keyword (which should be set to a dictionary of options)."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "a201e33c",
"metadata": {},
"outputs": [],
"source": [
"# Add input constraint, input cost, terminal cost\n",
"constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]\n",
"traj_cost = opt.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)\n",
"term_cost = opt.quadratic_cost(vehicle, np.diag([1, 10, 100]), None, x0=xf)\n",
"\n",
"# Define the time horizon (and spacing) for the optimization\n",
"horizon = np.linspace(0, Tf, 20, endpoint=True)\n",
"bend_left = [10, 0.001] # smaller left veer\n",
"\n",
"# Compute the optimal control\n",
"start_time = time.process_time()\n",
"result2 = opt.solve_ocp(\n",
" vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost,\n",
" initial_guess=bend_left, log=True,\n",
"# minimize_method='trust-constr',\n",
"# minimize_options={'finite_diff_rel_step': 0.01},\n",
" minimize_method='SLSQP', minimize_options={'eps': 0.01}\n",
")\n",
"print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n",
"\n",
"# Extract and plot the results (+ state trajectory)\n",
"t2, u2 = result2.time, result2.inputs\n",
"t2, y2 = ct.input_output_response(vehicle, horizon, u2, x0)\n",
"plot_lanechange(t2, y2, u2, yf=xf[0:2])\n",
"print(\"Final state: \", y2[:,-1])"
]
},
{
"cell_type": "markdown",
"id": "35d4faa6",
"metadata": {},
"source": [
"Note: you may get a warning from the solver about `delta_grad == 0.0`. I don't know quite why this happens."
]
},
{
"cell_type": "markdown",
"id": "3d2ccf97",
"metadata": {},
"source": [
"### Approach 3: terminal constraints\n",
"\n",
"We can also remove the cost function on the state and replace it with a terminal *constraint* on the state. If a solution is found, it guarantees we get to exactly the final state. Note however, that terminal constraints can be very difficult to satisfy for a general optimization (compare the solution times here with what we saw last week when we used differential flatness)."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "dc77a856",
"metadata": {},
"outputs": [],
"source": [
"# Input cost and terminal constraints\n",
"R = np.diag([1, 1]) # minimize applied inputs\n",
"cost3 = opt.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf)\n",
"constraints = [\n",
" opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]\n",
"terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]\n",
"\n",
"# Define the time horizon (and spacing) for the optimization\n",
"horizon = np.linspace(0, Tf, 12, endpoint=True)\n",
"\n",
"# Compute the optimal control\n",
"start_time = time.process_time()\n",
"result3 = opt.solve_ocp(\n",
" vehicle, horizon, x0, cost3, constraints,\n",
" terminal_constraints=terminal, initial_guess=bend_left, log=False,\n",
"# solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n",
"# minimize_method='trust-constr',\n",
"# minimize_options={'finite_diff_rel_step': 0.01},\n",
")\n",
"print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n",
"\n",
"# Extract and plot the results (+ state trajectory)\n",
"t3, u3 = result3.time, result3.inputs\n",
"t3, y3 = ct.input_output_response(vehicle, horizon, u3, x0)\n",
"plot_lanechange(t3, y3, u3, yf=xf[0:2])\n",
"print(\"Final state: \", y3[:,-1])"
]
},
{
"cell_type": "markdown",
"id": "9e744463",
"metadata": {},
"source": [
"### Approach 4: terminal constraints w/ basis functions\n",
"\n",
"As a final example, we can use a basis function to reduce the size\n",
"of the problem and get faster answers with more temporal resolution.\n",
"\n",
"Here we parameterize the input by a set of 4 Bezier curves but solve for a much more time resolved set of inputs. Note that while we are using the `control.flatsys` module to define the basis functions, we are not exploiting the fact that the system is differentially flat."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "ee82aa25",
"metadata": {},
"outputs": [],
"source": [
"# Get basis functions for flat systems module\n",
"import control.flatsys as flat\n",
"\n",
"# Compute the optimal control\n",
"start_time = time.process_time()\n",
"result4 = opt.solve_ocp(\n",
" vehicle, horizon, x0, quad_cost,\n",
" constraints,\n",
" terminal_constraints=terminal,\n",
" initial_guess=bend_left,\n",
" basis=flat.BezierFamily(6, T=Tf),\n",
" # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},\n",
" # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},\n",
" # minimize_method='trust-constr', minimize_options={'disp': True},\n",
" log=False\n",
")\n",
"print(\"* Total time = %5g seconds\\n\" % (time.process_time() - start_time))\n",
"\n",
"# Extract and plot the results (+ state trajectory)\n",
"t4, u4 = result4.time, result4.inputs\n",
"t4, y4 = ct.input_output_response(vehicle, horizon, u4, x0)\n",
"plot_lanechange(t4, y4, u4, yf=xf[0:2])\n",
"print(\"Final state: \", y4[:,-1])"
]
},
{
"cell_type": "markdown",
"id": "2a74388e",
"metadata": {},
"source": [
"Note how much smoother the inputs look, although the solver still has a hard time satisfying the final constraints."
]
},
{
"cell_type": "markdown",
"id": "1465d149",
"metadata": {},
"source": [
"### Additional things to try\n",
"\n",
"* Compare the results here with what we go last week exploiting the property of differential flatness (computation time, in particular)\n",
"* Try using different weights, solvers, initial guess and other properties and see how things change.\n",
"* Try using different values for `initial_guess` to get faster convergence and/or different classes of solutions."
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "02bad3d5",
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}