# Optimal control for vehicle steeering (lane change)¶

## Code¶

```  1# steering-optimal.py - optimal control for vehicle steering
2# RMM, 18 Feb 2021
3#
4# This file works through an optimal control example for the vehicle
5# steering system.  It is intended to demonstrate the functionality for
6# optimal control module (control.optimal) in the python-control package.
7
8import numpy as np
9import math
10import control as ct
11import control.optimal as opt
12import matplotlib.pyplot as plt
13import logging
14import time
15import os
16
17#
18# Vehicle steering dynamics
19#
20# The vehicle dynamics are given by a simple bicycle model.  We take the state
21# of the system as (x, y, theta) where (x, y) is the position of the vehicle
22# in the plane and theta is the angle of the vehicle with respect to
23# horizontal.  The vehicle input is given by (v, phi) where v is the forward
24# velocity of the vehicle and phi is the angle of the steering wheel.  The
25# model includes saturation of the vehicle steering angle.
26#
27# System state: x, y, theta
28# System input: v, phi
29# System output: x, y
30# System parameters: wheelbase, maxsteer
31#
32def vehicle_update(t, x, u, params):
33    # Get the parameters for the model
34    l = params.get('wheelbase', 3.)         # vehicle wheelbase
35    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)
36
37    # Saturate the steering input (use min/max instead of clip for speed)
38    phi = max(-phimax, min(u, phimax))
39
40    # Return the derivative of the state
41    return np.array([
42        math.cos(x) * u,            # xdot = cos(theta) v
43        math.sin(x) * u,            # ydot = sin(theta) v
44        (u / l) * math.tan(phi)        # thdot = v/l tan(phi)
45    ])
46
47
48def vehicle_output(t, x, u, params):
49    return x                            # return x, y, theta (full state)
50
51# Define the vehicle steering dynamics as an input/output system
52vehicle = ct.NonlinearIOSystem(
53    vehicle_update, vehicle_output, states=3, name='vehicle',
54    inputs=('v', 'phi'),
55    outputs=('x', 'y', 'theta'))
56
57#
58# Utility function to plot the results
59#
60def plot_results(t, y, u, figure=None, yf=None):
61    plt.figure(figure)
62
63    # Plot the xy trajectory
64    plt.subplot(3, 1, 1)
65    plt.plot(y, y)
66    plt.xlabel("x [m]")
67    plt.ylabel("y [m]")
68    if yf:
69        plt.plot(yf, yf, 'ro')
70
71    # Plot the inputs as a function of time
72    plt.subplot(3, 1, 2)
73    plt.plot(t, u)
74    plt.xlabel("t [sec]")
75    plt.ylabel("velocity [m/s]")
76
77    plt.subplot(3, 1, 3)
78    plt.plot(t, u)
79    plt.xlabel("t [sec]")
81
82    plt.suptitle("Lane change manuever")
83    plt.tight_layout()
84    plt.show(block=False)
85
86#
87# Optimal control problem
88#
89# Perform a "lane change" manuever over the course of 10 seconds.
90#
91
92# Initial and final conditions
93x0 = [0., -2., 0.]; u0 = [10., 0.]
94xf = [100., 2., 0.]; uf = [10., 0.]
95Tf = 10
96
97#
98# Approach 1: standard quadratic cost
99#
100# We can set up the optimal control problem as trying to minimize the
101# distance form the desired final point while at the same time as not
102# exerting too much control effort to achieve our goal.
103#
104print("Approach 1: standard quadratic cost")
105
106# Set up the cost functions
107Q = np.diag([.1, 10, .1])       # keep lateral error low
108R = np.diag([.1, 1])            # minimize applied inputs
109quad_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
110
111# Define the time horizon (and spacing) for the optimization
112horizon = np.linspace(0, Tf, 10, endpoint=True)
113
114# Provide an intial guess (will be extended to entire horizon)
115bend_left = [10, 0.01]          # slight left veer
116
117# Turn on debug level logging so that we can see what the optimizer is doing
118logging.basicConfig(
119    level=logging.DEBUG, filename="steering-integral_cost.log",
120    filemode='w', force=True)
121
122# Compute the optimal control, setting step size for gradient calculation (eps)
123start_time = time.process_time()
124result1 = opt.solve_ocp(
125    vehicle, horizon, x0, quad_cost, initial_guess=bend_left, log=True,
126    minimize_method='trust-constr',
127    minimize_options={'finite_diff_rel_step': 0.01},
128)
129print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
130
131# If we are running CI tests, make sure we succeeded
132if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
133    assert result1.success
134
135# Extract and plot the results (+ state trajectory)
136t1, u1 = result1.time, result1.inputs
137t1, y1 = ct.input_output_response(vehicle, horizon, u1, x0)
138plot_results(t1, y1, u1, figure=1, yf=xf[0:2])
139
140#
141# Approach 2: input cost, input constraints, terminal cost
142#
143# The previous solution integrates the position error for the entire
144# horizon, and so the car changes lanes very quickly (at the cost of larger
145# inputs).  Instead, we can penalize the final state and impose a higher
146# cost on the inputs, resuling in a more graduate lane change.
147#
148# We also set the solver explicitly.
149#
150print("Approach 2: input cost and constraints plus terminal cost")
151
152# Add input constraint, input cost, terminal cost
153constraints = [ opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
154traj_cost = opt.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)
155term_cost = opt.quadratic_cost(vehicle, np.diag([1, 10, 10]), None, x0=xf)
156
157# Change logging to keep less information
158logging.basicConfig(
159    level=logging.INFO, filename="./steering-terminal_cost.log",
160    filemode='w', force=True)
161
162# Compute the optimal control
163start_time = time.process_time()
164result2 = opt.solve_ocp(
165    vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost,
166    initial_guess=bend_left, log=True,
167    minimize_method='SLSQP', minimize_options={'eps': 0.01})
168print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
169
170# If we are running CI tests, make sure we succeeded
171if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
172    assert result2.success
173
174# Extract and plot the results (+ state trajectory)
175t2, u2 = result2.time, result2.inputs
176t2, y2 = ct.input_output_response(vehicle, horizon, u2, x0)
177plot_results(t2, y2, u2, figure=2, yf=xf[0:2])
178
179#
180# Approach 3: terminal constraints
181#
182# We can also remove the cost function on the state and replace it
183# with a terminal *constraint* on the state.  If a solution is found,
184# it guarantees we get to exactly the final state.
185#
186print("Approach 3: terminal constraints")
187
188# Input cost and terminal constraints
189R = np.diag([1, 1])                 # minimize applied inputs
190cost3 = opt.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf)
191constraints = [
192    opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
193terminal = [ opt.state_range_constraint(vehicle, xf, xf) ]
194
195# Reset logging to its default values
196logging.basicConfig(
197    level=logging.DEBUG, filename="./steering-terminal_constraint.log",
198    filemode='w', force=True)
199
200# Compute the optimal control
201start_time = time.process_time()
202result3 = opt.solve_ocp(
203    vehicle, horizon, x0, cost3, constraints,
204    terminal_constraints=terminal, initial_guess=bend_left, log=False,
205    solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},
206    minimize_method='trust-constr',
207)
208print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
209
210# If we are running CI tests, make sure we succeeded
211if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
212    assert result3.success
213
214# Extract and plot the results (+ state trajectory)
215t3, u3 = result3.time, result3.inputs
216t3, y3 = ct.input_output_response(vehicle, horizon, u3, x0)
217plot_results(t3, y3, u3, figure=3, yf=xf[0:2])
218
219#
220# Approach 4: terminal constraints w/ basis functions
221#
222# As a final example, we can use a basis function to reduce the size
223# of the problem and get faster answers with more temporal resolution.
224# Here we parameterize the input by a set of 4 Bezier curves but solve
225# for a much more time resolved set of inputs.
226
227print("Approach 4: Bezier basis")
228import control.flatsys as flat
229
230# Compute the optimal control
231start_time = time.process_time()
232result4 = opt.solve_ocp(
233    vehicle, horizon, x0, quad_cost,
234    constraints,
235    terminal_constraints=terminal,
236    initial_guess=bend_left,
237    basis=flat.BezierFamily(4, T=Tf),
238    # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},
239    solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},
240    minimize_method='trust-constr', minimize_options={'disp': True},
241    log=False
242)
243print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
244
245# If we are running CI tests, make sure we succeeded
246if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
247    assert result4.success
248
249# Extract and plot the results (+ state trajectory)
250t4, u4 = result4.time, result4.inputs
251t4, y4 = ct.input_output_response(vehicle, horizon, u4, x0)
252plot_results(t4, y4, u4, figure=4, yf=xf[0:2])
253
254# If we are not running CI tests, display the results
255if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
256    plt.show()
```