# Optimal control for vehicle steering (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 obc
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[1], phimax))
39
40    # Return the derivative of the state
41    return np.array([
42        math.cos(x[2]) * u[0],            # xdot = cos(theta) v
43        math.sin(x[2]) * u[0],            # ydot = sin(theta) v
44        (u[0] / 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_lanechange(t, y, u, yf=None, figure=None):
61    plt.figure(figure)
62
63    # Plot the xy trajectory
64    plt.subplot(3, 1, 1)
65    plt.plot(y[0], y[1])
66    plt.xlabel("x [m]")
67    plt.ylabel("y [m]")
68    if yf is not None:
69        plt.plot(yf[0], yf[1], 'ro')
70
71    # Plot the inputs as a function of time
72    plt.subplot(3, 1, 2)
73    plt.plot(t, u[0])
74    plt.xlabel("t [sec]")
75    plt.ylabel("velocity [m/s]")
76
77    plt.subplot(3, 1, 3)
78    plt.plot(t, u[1])
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 = np.array([0., -2., 0.]); u0 = np.array([10., 0.])
94xf = np.array([100., 2., 0.]); uf = np.array([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#
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
110
111# Define the time horizon (and spacing) for the optimization
112timepts = np.linspace(0, Tf, 20, endpoint=True)
113
114# Provide an initial guess
115straight_line = (
116    np.array([x0 + (xf - x0) * time/Tf for time in timepts]).transpose(),
117    np.outer(u0, np.ones_like(timepts))
118)
119
120# Turn on debug level logging so that we can see what the optimizer is doing
121logging.basicConfig(
122    level=logging.DEBUG, filename="steering-integral_cost.log",
123    filemode='w', force=True)
124
125# Compute the optimal control, setting step size for gradient calculation (eps)
126start_time = time.process_time()
127result1 = obc.solve_ocp(
128    vehicle, timepts, x0, quad_cost, initial_guess=straight_line, log=True,
129    # minimize_method='trust-constr',
130    # minimize_options={'finite_diff_rel_step': 0.01},
131)
132print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
133
134# If we are running CI tests, make sure we succeeded
135if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
136    assert result1.success
137
138# Plot the results from the optimization
139plot_lanechange(timepts, result1.states, result1.inputs, xf, figure=1)
140print("Final computed state: ", result1.states[:,-1])
141
142# Simulate the system and see what happens
143t1, u1 = result1.time, result1.inputs
144t1, y1 = ct.input_output_response(vehicle, timepts, u1, x0)
145plot_lanechange(t1, y1, u1, yf=xf[0:2], figure=1)
146print("Final simulated state:", y1[:,-1])
147
148#
149# Approach 2: input cost, input constraints, terminal cost
150#
151# The previous solution integrates the position error for the entire
152# horizon, and so the car changes lanes very quickly (at the cost of larger
153# inputs).  Instead, we can penalize the final state and impose a higher
154# cost on the inputs, resuling in a more graduate lane change.
155#
156# We also set the solver explicitly.
157#
158print("\nApproach 2: input cost and constraints plus terminal cost")
159
160# Add input constraint, input cost, terminal cost
161constraints = [ obc.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
162traj_cost = obc.quadratic_cost(vehicle, None, np.diag([0.1, 1]), u0=uf)
163term_cost = obc.quadratic_cost(vehicle, np.diag([1, 10, 10]), None, x0=xf)
164
165# Change logging to keep less information
166logging.basicConfig(
167    level=logging.INFO, filename="./steering-terminal_cost.log",
168    filemode='w', force=True)
169
170# Use a straight line between initial and final position as initial guesss
171input_guess = np.outer(u0, np.ones((1, timepts.size)))
172state_guess = np.array([
173    x0 + (xf - x0) * time/Tf for time in timepts]).transpose()
174straight_line = (state_guess, input_guess)
175
176# Compute the optimal control
177start_time = time.process_time()
178result2 = obc.solve_ocp(
179    vehicle, timepts, x0, traj_cost, constraints, terminal_cost=term_cost,
180    initial_guess=straight_line, log=True,
181    # minimize_method='SLSQP', minimize_options={'eps': 0.01}
182)
183print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
184
185# If we are running CI tests, make sure we succeeded
186if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
187    assert result2.success
188
189# Plot the results from the optimization
190plot_lanechange(timepts, result2.states, result2.inputs, xf, figure=2)
191print("Final computed state: ", result2.states[:,-1])
192
193# Simulate the system and see what happens
194t2, u2 = result2.time, result2.inputs
195t2, y2 = ct.input_output_response(vehicle, timepts, u2, x0)
196plot_lanechange(t2, y2, u2, yf=xf[0:2], figure=2)
197print("Final simulated state:", y2[:,-1])
198
199#
200# Approach 3: terminal constraints
201#
202# We can also remove the cost function on the state and replace it
203# with a terminal *constraint* on the state.  If a solution is found,
204# it guarantees we get to exactly the final state.
205#
206print("\nApproach 3: terminal constraints")
207
208# Input cost and terminal constraints
209R = np.diag([1, 1])                 # minimize applied inputs
210cost3 = obc.quadratic_cost(vehicle, np.zeros((3,3)), R, u0=uf)
211constraints = [
212    obc.input_range_constraint(vehicle, [8, -0.1], [12, 0.1]) ]
213terminal = [ obc.state_range_constraint(vehicle, xf, xf) ]
214
215# Reset logging to its default values
216logging.basicConfig(
217    level=logging.DEBUG, filename="./steering-terminal_constraint.log",
218    filemode='w', force=True)
219
220# Compute the optimal control
221start_time = time.process_time()
222result3 = obc.solve_ocp(
223    vehicle, timepts, x0, cost3, constraints,
224    terminal_constraints=terminal, initial_guess=straight_line, log=False,
225    # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},
226    # minimize_method='trust-constr',
227)
228print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
229
230# If we are running CI tests, make sure we succeeded
231if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
232    assert result3.success
233
234# Plot the results from the optimization
235plot_lanechange(timepts, result3.states, result3.inputs, xf, figure=3)
236print("Final computed state: ", result3.states[:,-1])
237
238# Simulate the system and see what happens
239t3, u3 = result3.time, result3.inputs
240t3, y3 = ct.input_output_response(vehicle, timepts, u3, x0)
241plot_lanechange(t3, y3, u3, yf=xf[0:2], figure=3)
242print("Final simulated state:", y3[:,-1])
243
244#
245# Approach 4: terminal constraints w/ basis functions
246#
247# As a final example, we can use a basis function to reduce the size
248# of the problem and get faster answers with more temporal resolution.
249# Here we parameterize the input by a set of 4 Bezier curves but solve
250# for a much more time resolved set of inputs.
251
252print("\nApproach 4: Bezier basis")
253import control.flatsys as flat
254
255# Compute the optimal control
256start_time = time.process_time()
257result4 = obc.solve_ocp(
259    constraints,
260    terminal_constraints=terminal,
261    initial_guess=straight_line,
262    basis=flat.BezierFamily(6, T=Tf),
263    # solve_ivp_kwargs={'method': 'RK45', 'atol': 1e-2, 'rtol': 1e-2},
264    # solve_ivp_kwargs={'atol': 1e-3, 'rtol': 1e-2},
265    # minimize_method='trust-constr', minimize_options={'disp': True},
266    log=False
267)
268print("* Total time = %5g seconds\n" % (time.process_time() - start_time))
269
270# If we are running CI tests, make sure we succeeded
271if 'PYCONTROL_TEST_EXAMPLES' in os.environ:
272    assert result4.success
273
274# Plot the results from the optimization
275plot_lanechange(timepts, result4.states, result4.inputs, xf, figure=4)
276print("Final computed state: ", result3.states[:,-1])
277
278# Simulate the system and see what happens
279t4, u4 = result4.time, result4.inputs
280t4, y4 = ct.input_output_response(vehicle, timepts, u4, x0)
281plot_lanechange(t4, y4, u4, yf=xf[0:2], figure=4)
282print("Final simulated state: ", y4[:,-1])
283
284# If we are not running CI tests, display the results
285if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
286    plt.show()
```