Differentially flat system - kinematic car¶
This example demonstrates the use of the flatsys module for generating trajectories for differentially flat systems. The example is drawn from Chapter 8 of FBS2e.
Code¶
1# kincar-flatsys.py - differentially flat systems example
2# RMM, 3 Jul 2019
3#
4# This example demonstrates the use of the `flatsys` module for generating
5# trajectories for differnetially flat systems by computing a trajectory for a
6# kinematic (bicycle) model of a car changing lanes.
7
8import os
9import numpy as np
10import matplotlib.pyplot as plt
11import control as ct
12import control.flatsys as fs
13import control.optimal as obc
14
15#
16# System model and utility functions
17#
18
19# Function to take states, inputs and return the flat flag
20def vehicle_flat_forward(x, u, params={}):
21 # Get the parameter values
22 b = params.get('wheelbase', 3.)
23
24 # Create a list of arrays to store the flat output and its derivatives
25 zflag = [np.zeros(3), np.zeros(3)]
26
27 # Flat output is the x, y position of the rear wheels
28 zflag[0][0] = x[0]
29 zflag[1][0] = x[1]
30
31 # First derivatives of the flat output
32 zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt
33 zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt
34
35 # First derivative of the angle
36 thdot = (u[0]/b) * np.tan(u[1])
37
38 # Second derivatives of the flat output (setting vdot = 0)
39 zflag[0][2] = -u[0] * thdot * np.sin(x[2])
40 zflag[1][2] = u[0] * thdot * np.cos(x[2])
41
42 return zflag
43
44
45# Function to take the flat flag and return states, inputs
46def vehicle_flat_reverse(zflag, params={}):
47 # Get the parameter values
48 b = params.get('wheelbase', 3.)
49
50 # Create a vector to store the state and inputs
51 x = np.zeros(3)
52 u = np.zeros(2)
53
54 # Given the flat variables, solve for the state
55 x[0] = zflag[0][0] # x position
56 x[1] = zflag[1][0] # y position
57 x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot
58
59 # And next solve for the inputs
60 u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
61 thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
62 u[1] = np.arctan2(thdot_v, u[0]**2 / b)
63
64 return x, u
65
66# Function to compute the RHS of the system dynamics
67def vehicle_update(t, x, u, params):
68 b = params.get('wheelbase', 3.) # get parameter values
69 dx = np.array([
70 np.cos(x[2]) * u[0],
71 np.sin(x[2]) * u[0],
72 (u[0]/b) * np.tan(u[1])
73 ])
74 return dx
75
76# Plot the trajectory in xy coordinates
77def plot_results(t, x, ud, rescale=True):
78 plt.subplot(4, 1, 2)
79 plt.plot(x[0], x[1])
80 plt.xlabel('x [m]')
81 plt.ylabel('y [m]')
82 if rescale:
83 plt.axis([x0[0], xf[0], x0[1]-1, xf[1]+1])
84
85 # Time traces of the state and input
86 plt.subplot(2, 4, 5)
87 plt.plot(t, x[1])
88 plt.ylabel('y [m]')
89
90 plt.subplot(2, 4, 6)
91 plt.plot(t, x[2])
92 plt.ylabel('theta [rad]')
93
94 plt.subplot(2, 4, 7)
95 plt.plot(t, ud[0])
96 plt.xlabel('Time t [sec]')
97 plt.ylabel('v [m/s]')
98 if rescale:
99 plt.axis([0, Tf, u0[0] - 1, uf[0] + 1])
100
101 plt.subplot(2, 4, 8)
102 plt.plot(t, ud[1])
103 plt.xlabel('Ttime t [sec]')
104 plt.ylabel('$\delta$ [rad]')
105 plt.tight_layout()
106
107#
108# Approach 1: point to point solution, no cost or constraints
109#
110
111# Create differentially flat input/output system
112vehicle_flat = fs.FlatSystem(
113 vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
114 inputs=('v', 'delta'), outputs=('x', 'y'),
115 states=('x', 'y', 'theta'))
116
117# Define the endpoints of the trajectory
118x0 = [0., -2., 0.]; u0 = [10., 0.]
119xf = [40., 2., 0.]; uf = [10., 0.]
120Tf = 4
121
122# Define a set of basis functions to use for the trajectories
123poly = fs.PolyFamily(6)
124
125# Find a trajectory between the initial condition and the final condition
126traj1 = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
127
128# Create the desired trajectory between the initial and final condition
129T = np.linspace(0, Tf, 500)
130xd, ud = traj1.eval(T)
131
132# Simulation the open system dynamics with the full input
133t, y, x = ct.input_output_response(
134 vehicle_flat, T, ud, x0, return_x=True)
135
136# Plot the open loop system dynamics
137plt.figure(1)
138plt.suptitle("Open loop trajectory for kinematic car lane change")
139plot_results(t, x, ud)
140
141#
142# Approach #2: add cost function to make lane change quicker
143#
144
145# Define timepoints for evaluation plus basis function to use
146timepts = np.linspace(0, Tf, 10)
147basis = fs.PolyFamily(8)
148
149# Define the cost function (penalize lateral error and steering)
150traj_cost = obc.quadratic_cost(
151 vehicle_flat, np.diag([0, 0.1, 0]), np.diag([0.1, 1]), x0=xf, u0=uf)
152
153# Solve for an optimal solution
154traj2 = fs.point_to_point(
155 vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost, basis=basis,
156)
157xd, ud = traj2.eval(T)
158
159plt.figure(2)
160plt.suptitle("Lane change with lateral error + steering penalties")
161plot_results(T, xd, ud)
162
163#
164# Approach #3: optimal cost with trajectory constraints
165#
166# Resolve the problem with constraints on the inputs
167#
168
169# Constraint the input values
170constraints = [
171 obc.input_range_constraint(vehicle_flat, [8, -0.1], [12, 0.1]) ]
172
173# TEST: Change the basis to use B-splines
174basis = fs.BSplineFamily([0, Tf/2, Tf], 6)
175
176# Solve for an optimal solution
177traj3 = fs.point_to_point(
178 vehicle_flat, timepts, x0, u0, xf, uf, cost=traj_cost,
179 constraints=constraints, basis=basis,
180)
181xd, ud = traj3.eval(T)
182
183plt.figure(3)
184plt.suptitle("Lane change with penalty + steering constraints")
185plot_results(T, xd, ud)
186
187# Show the results unless we are running in batch mode
188if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
189 plt.show()
190
191
192#
193# Approach #4: optimal trajectory, final cost with trajectory constraints
194#
195# Resolve the problem with constraints on the inputs and also replacing the
196# point to point problem with one using a terminal cost to set the final
197# state.
198#
199
200# Define the cost function (mainly penalize steering angle)
201traj_cost = obc.quadratic_cost(
202 vehicle_flat, None, np.diag([0.1, 10]), x0=xf, u0=uf)
203
204# Set terminal cost to bring us close to xf
205terminal_cost = obc.quadratic_cost(vehicle_flat, 1e3 * np.eye(3), None, x0=xf)
206
207# Change the basis to use B-splines
208basis = fs.BSplineFamily([0, Tf/2, Tf], [4, 6], vars=2)
209
210# Use a straight line as an initial guess for the trajectory
211initial_guess = np.array(
212 [x0[i] + (xf[i] - x0[i]) * timepts/Tf for i in (0, 1)])
213
214# Solve for an optimal solution
215traj4 = fs.solve_flat_ocp(
216 vehicle_flat, timepts, x0, u0, cost=traj_cost, constraints=constraints,
217 terminal_cost=terminal_cost, basis=basis, initial_guess=initial_guess,
218 # minimize_kwargs={'method': 'trust-constr'},
219)
220xd, ud = traj4.eval(T)
221
222plt.figure(4)
223plt.suptitle("Lane change with terminal cost + steering constraints")
224plot_results(T, xd, ud, rescale=False) # TODO: remove rescale
225
226# Show the results unless we are running in batch mode
227if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
228 plt.show()
Notes¶
1. The environment variable PYCONTROL_TEST_EXAMPLES is used for testing to turn off plotting of the outputs.