RMM comments to Karl, 27 Jun 2019
In [1]:
import numpy as np
import matplotlib.pyplot as plt
import control as ct
ct.use_fbs_defaults()
ct.use_numpy_matrix(False)
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 reference point 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, \delta)$ where $v$ is the forward velocity of the vehicle and $\delta$ is the angle of the steering wheel. We take as parameters the wheelbase $b$ and the offset $a$ between the rear wheels and the reference point. The model includes saturation of the vehicle steering angle (maxsteer
).
x
, y
, theta
v
, delta
x
, y
wheelbase
, refoffset
, maxsteer
Assuming no slipping of the wheels, the motion of the vehicle is given by a rotation around a point O that depends on the steering angle $\delta$. To compute the angle $\alpha$ of the velocity of the reference point with respect to the axis of the vehicle, we let the distance from the center of rotation O to the contact point of the rear wheel be $r_\text{r}$ and it the follows from Figure 3.17 in FBS that $b = r_\text{r} \tan \delta$ and $a = r_\text{r} \tan \alpha$, which implies that $\tan \alpha = (a/b) \tan \delta$.
Reasonable limits for the steering angle depend on the speed. The physical limit is given in our model as 0.5 radians (about 30 degrees). However, this limit is rarely possible when the car is driving since it would cause the tires to slide on the pavement. We us a limit of 0.1 radians (about 6 degrees) at 10 m/s ($\approx$ 35 kph) and 0.05 radians (about 3 degrees) at 30 m/s ($\approx$ 110 kph). Note that a steering angle of 0.05 rad gives a cross acceleration of $(v^2/b) \tan \delta \approx (100/3) 0.05 = 1.7$ $\text{m/s}^2$ at 10 m/s and 15 $\text{m/s}^2$ at 30 m/s ($\approx$ 1.5 times the force of gravity).
In [2]:
def vehicle_update(t, x, u, params):
# Get the parameters for the model
a = params.get('refoffset', 1.5) # offset to vehicle reference point
b = params.get('wheelbase', 3.) # vehicle wheelbase
maxsteer = params.get('maxsteer', 0.5) # max steering angle (rad)
# Saturate the steering input
delta = np.clip(u[1], -maxsteer, maxsteer)
alpha = np.arctan2(a * np.tan(delta), b)
# Return the derivative of the state
return np.array([
u[0] * np.cos(x[2] + alpha), # xdot = cos(theta + alpha) v
u[0] * np.sin(x[2] + alpha), # ydot = sin(theta + alpha) v
(u[0] / b) * np.tan(delta) # thdot = v/l tan(phi)
])
def vehicle_output(t, x, u, params):
return x[0:2]
# Default vehicle parameters (including nominal velocity)
vehicle_params={'refoffset': 1.5, 'wheelbase': 3, 'velocity': 15,
'maxsteer': 0.5}
# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
vehicle_update, vehicle_output, states=3, name='vehicle',
inputs=('v', 'delta'), outputs=('x', 'y'), params=vehicle_params)
RMM notes, 27 Jun 2019:
OutputFeedback/figures/steering_obs.m
.KJA comments, 1 Jul 2019:
I think we should point out that the reference point is typically the projection of the center of mass of the whole vehicle.
The heading angle $\theta$ must be marked in Figure 3.17b.
I think it is useful to start with a curvy road that you have done here but then to specialized to a trajectory that is essentially horizontal, where $y$ is the deviation from the nominal horizontal $x$ axis. Assuming that $\alpha$ and $\theta$ are small we get the natural linearization of (3.26) $\dot x = v$ and $\dot y =v(\alpha + \theta)$
RMM response, 16 Jul 2019:
KJA response, 20 Jul 2019: Fig 8.6a is fine
In [3]:
# System parameters
wheelbase = vehicle_params['wheelbase']
v0 = vehicle_params['velocity']
# Control inputs
T_curvy = np.linspace(0, 7, 500)
v_curvy = v0*np.ones(T_curvy.shape)
delta_curvy = 0.1*np.sin(T_curvy)*np.cos(4*T_curvy) + 0.0025*np.sin(T_curvy*np.pi/7)
u_curvy = [v_curvy, delta_curvy]
X0_curvy = [0, 0.8, 0]
# Simulate the system + estimator
t_curvy, y_curvy, x_curvy = ct.input_output_response(
vehicle, T_curvy, u_curvy, X0_curvy, params=vehicle_params, return_x=True)
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the resulting trajectory (and some road boundaries)
plt.subplot(1, 4, 2)
plt.plot(y_curvy[1], y_curvy[0])
plt.plot(y_curvy[1] - 9/np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)
plt.plot(y_curvy[1] - 3/np.cos(x_curvy[2]), y_curvy[0], 'k--', linewidth=1)
plt.plot(y_curvy[1] + 3/np.cos(x_curvy[2]), y_curvy[0], 'k-', linewidth=1)
plt.xlabel('y [m]')
plt.ylabel('x [m]');
plt.axis('Equal')
# Plot the lateral position
plt.subplot(2, 2, 2)
plt.plot(t_curvy, y_curvy[1])
plt.ylabel('Lateral position $y$ [m]')
# Plot the steering angle
plt.subplot(2, 2, 4)
plt.plot(t_curvy, delta_curvy)
plt.ylabel('Steering angle $\\delta$ [rad]')
plt.xlabel('Time t [sec]')
plt.tight_layout()
We are interested in the motion of the vehicle about a straight-line path ($\theta = \theta_0$) with constant velocity $v_0 \neq 0$. To find the relevant equilibrium point, we first set $\dot\theta = 0$ and we see that we must have $\delta = 0$, corresponding to the steering wheel being straight. The motion in the xy plane is by definition not at equilibrium and so we focus on lateral deviation of the vehicle from a straight line. For simplicity, we let $\theta_\text{e} = 0$, which corresponds to driving along the $x$ axis. We can then focus on the equations of motion in the $y$ and $\theta$ directions with input $u = \delta$.
In [4]:
# Define the lateral dynamics as a subset of the full vehicle steering dynamics
lateral = ct.NonlinearIOSystem(
lambda t, x, u, params: vehicle_update(
t, [0., x[0], x[1]], [params.get('velocity', 1), u[0]], params)[1:],
lambda t, x, u, params: vehicle_output(
t, [0., x[0], x[1]], [params.get('velocity', 1), u[0]], params)[1:],
states=2, name='lateral', inputs=('phi'), outputs=('y')
)
# Compute the linearization at velocity v0 = 15 m/sec
lateral_linearized = ct.linearize(lateral, [0, 0], [0], params=vehicle_params)
# Normalize dynamics using state [x1/b, x2] and timescale v0 t / b
b = vehicle_params['wheelbase']
v0 = vehicle_params['velocity']
lateral_transformed = ct.similarity_transform(
lateral_linearized, [[1/b, 0], [0, 1]], timescale=v0/b)
# Set the output to be the normalized state x1/b
lateral_normalized = lateral_transformed * (1/b)
print("Linearized system dynamics:\n")
print(lateral_normalized)
# Save the system matrices for later use
A = lateral_normalized.A
B = lateral_normalized.B
C = lateral_normalized.C
We want to design a controller that stabilizes the dynamics of the vehicle and tracks a given reference value $r$ of the lateral position of the vehicle. We use feedback to design the dynamics of the system to have the characteristic polynomial $p(s) = s^2 + 2 \zeta_\text{c} \omega_\text{c} + \omega_\text{c}^2$.
To find reasonable values of $\omega_\text{c}$ we observe that the initial response of the steering angle to a unit step change in the steering command is $\omega_\text{c}^2 r$, where $r$ is the commanded lateral transition. Recall that the model is normalized so that the length unit is the wheelbase $b$ and the time unit is the time $b/v_0$ to travel one wheelbase. A typical car has a wheelbase of about 3 m and, assuming a speed of 30 m/s, a normalized time unit corresponds to 0.1 s. To determine a reasonable steering angle when making a gentle lane change, we assume that the turning radius is $R$ = 600 m. For a wheelbase of 3 m this corresponds to a steering angle $\delta \approx 3/600 = 0.005$ rad and a lateral acceleration of $v^2/R$ = 302/600 = 1.5 m/s$^2$. Assuming that a lane change corresponds to a translation of one wheelbase we find $\omega_\text{c} = \sqrt{0.005}$ = 0.07 rad/s.
The unit step responses for the closed loop system for different values of the design parameters are shown below. The effect of $\omega_c$ is shown on the left, which shows that the response speed increases with increasing $\omega_\text{c}$. All responses have overshoot less than 5% (15 cm), as indicated by the dashed lines. The settling times range from 30 to 60 normalized time units, which corresponds to about 3–6 s, and are limited by the acceptable lateral acceleration of the vehicle. The effect of $\zeta_\text{c}$ is shown on the right. The response speed and the overshoot increase with decreasing damping. Using these plots, we conclude that a reasonable design choice is $\omega_\text{c} = 0.07$ and $\zeta_\text{c} = 0.7$.
RMM note, 27 Jun 2019:
KJA comment, 1 Jul 2019:
RMM comment, 17 Jul 2019:
In [5]:
# Utility function to place poles for the normalized vehicle steering system
def normalized_place(wc, zc):
# Get the dynamics and input matrices, for later use
A, B = lateral_normalized.A, lateral_normalized.B
# Compute the eigenvalues from the characteristic polynomial
eigs = np.roots([1, 2*zc*wc, wc**2])
# Compute the feedback gain using eigenvalue placement
K = ct.place_varga(A, B, eigs)
# Create a new system representing the closed loop response
clsys = ct.StateSpace(A - B @ K, B, lateral_normalized.C, 0)
# Compute the feedforward gain based on the zero frequency gain of the closed loop
kf = np.real(1/clsys.evalfr(0))
# Scale the input by the feedforward gain
clsys *= kf
# Return gains and closed loop system dynamics
return K, kf, clsys
# Utility function to plot simulation results for normalized vehicle steering system
def normalized_plot(t, y, u, inpfig, outfig):
plt.sca(outfig)
plt.plot(t, y)
plt.sca(inpfig)
plt.plot(t, u[0])
# Utility function to label plots of normalized vehicle steering system
def normalized_label(inpfig, outfig):
plt.sca(inpfig)
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Steering angle $\delta$ [rad]')
plt.sca(outfig)
plt.ylabel('Lateral position $y/b$')
plt.plot([0, 20], [0.95, 0.95], 'k--')
plt.plot([0, 20], [1.05, 1.05], 'k--')
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Explore range of values for omega_c, with zeta_c = 0.7
outfig = plt.subplot(2, 2, 1)
inpfig = plt.subplot(2, 2, 3)
zc = 0.7
for wc in [0.5, 0.7, 1]:
# Place the poles of the system
K, kf, clsys = normalized_place(wc, zc)
# Compute the step response
t, y, x = ct.step_response(clsys, np.linspace(0, 20, 100), return_x=True)
# Compute the input used to generate the control response
u = -K @ x + kf * 1
# Plot the results
normalized_plot(t, y, u, inpfig, outfig)
# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\omega_c = 0.5$', '$\omega_c = 0.7$', '$\omega_c = 0.1$'))
# Explore range of values for zeta_c, with omega_c = 0.07
outfig = plt.subplot(2, 2, 2)
inpfig = plt.subplot(2, 2, 4)
wc = 0.7
for zc in [0.5, 0.7, 1]:
# Place the poles of the system
K, kf, clsys = normalized_place(wc, zc)
# Compute the step response
t, y, x = ct.step_response(clsys, np.linspace(0, 20, 100), return_x=True)
# Compute the input used to generate the control response
u = -K @ x + kf * 1
# Plot the results
normalized_plot(t, y, u, inpfig, outfig)
# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\zeta_c = 0.5$', '$\zeta_c = 0.7$', '$\zeta_c = 1$'))
plt.tight_layout()
RMM notes, 17 Jul 2019
KJA response, 20 Jul 2019: Makes a lot of sense to make $\omega_\text{c}$ range from 0.5 to 1 (10X faster). The plots were still in the range 0.05 to 0.1 in the note you sent me.
RMM response: 23 Jul 2019: Updated $\omega_\text{c}$ to 10X faster. Note that this makes size of the inputs for the step response quite large, but that is in part because a unit step in the desired position produces an (instantaneous) error of $b = 3$ m $\implies$ quite a large error. A lateral error of 10 cm with $\omega_c = 0.7$ would produce an (initial) input of 0.015 rad.
In [6]:
# Find the eigenvalue from the characteristic polynomial
wo = 1 # bandwidth for the observer
zo = 0.7 # damping ratio for the observer
eigs = np.roots([1, 2*zo*wo, wo**2])
# Compute the estimator gain using eigenvalue placement
L = np.transpose(
ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", L)
# Create a linear model of the lateral dynamics driving the estimator
est = ct.StateSpace(A - L @ C, np.block([[B, L]]), np.eye(2), np.zeros((2,2)))
A simulation of the observer for a vehicle driving on a curvy road is shown below. The first figure shows the trajectory of the vehicle on the road, as viewed from above. The response of the observer is shown on the right, where time is normalized to the vehicle length. We see that the observer error settles in about 4 vehicle lengths.
RMM note, 27 Jun 2019:
KJA comment, 1 Jul 2019:
RMM comment, 17 Jul 2019: plan to use this version in the text.
KJA comment, 20 Jul 2019: I think this is a big improvement we show that an observer based on a linearized model works on a nonlinear simulation, If possible we could add a line telling why the linear model works and that this is standard procedure in control engineering.
In [7]:
# Convert the curvy trajectory into normalized coordinates
x_ref = x_curvy[0] / wheelbase
y_ref = x_curvy[1] / wheelbase
theta_ref = x_curvy[2]
tau = v0 * T_curvy / b
# Simulate the estimator, with a small initial error in y position
t, y_est, x_est = ct.forced_response(est, tau, [delta_curvy, y_ref], [0.5, 0])
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the actual and estimated states
ax = plt.subplot(2, 2, 1)
plt.plot(t, y_ref)
plt.plot(t, x_est[0])
ax.set(xlim=[0, 10])
plt.legend(['actual', 'estimated'])
plt.ylabel('Lateral position $y/b$')
ax = plt.subplot(2, 2, 2)
plt.plot(t, x_est[0] - y_ref)
ax.set(xlim=[0, 10])
plt.ylabel('Lateral error')
ax = plt.subplot(2, 2, 3)
plt.plot(t, theta_ref)
plt.plot(t, x_est[1])
ax.set(xlim=[0, 10])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Vehicle angle $\\theta$')
ax = plt.subplot(2, 2, 4)
plt.plot(t, x_est[1] - theta_ref)
ax.set(xlim=[0, 10])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Angle error')
plt.tight_layout()
RMM note, 27 Jun 2019
theta_curvy
.KJA comment, 1 Jul 2019:
The large initial errors dominate the plots.
There is somehing funny happening at the end of the simulation, may be due to the small curvature at the end of the path?
RMM comment, 17 Jul 2019:
KJA comment, 20 Jul 2019: We may add that state feedback is a proportional controller which does not guarantee that the error goes to zero for example by changing the line "The tracking error ..." to "The tracking error can be improved by adding integral action (Section7.4), later in this chapter "Disturbance Modeling" or feedforward (Section 8,5). Should we do an exercises?
In [8]:
# Compute the feedback gains
# K, kf, clsys = normalized_place(1, 0.707) # Gains from MATLAB
# K, kf, clsys = normalized_place(0.07, 0.707) # Original gains
K, kf, clsys = normalized_place(0.7, 0.707) # Final gains
# Print out the gains
print("K = ", K)
print("kf = ", kf)
# Construct an output-based controller for the system
clsys = ct.StateSpace(
np.block([[A, -B@K], [L@C, A - B@K - L@C]]),
np.block([[B], [B]]) * kf,
np.block([[C, np.zeros(C.shape)], [np.zeros(C.shape), C]]),
np.zeros((2,1)))
# Simulate the system
t, y, x = ct.forced_response(clsys, tau, y_ref, [0.4, 0, 0.0, 0])
# Calcaluate the input used to generate the control response
u_sfb = kf * y_ref - K @ x[0:2]
u_ofb = kf * y_ref - K @ x[2:4]
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the actual and estimated states
ax = plt.subplot(1, 2, 1)
plt.plot(t, x[0])
plt.plot(t, x[2])
plt.plot(t, y_ref, 'k-.')
ax.set(xlim=[0, 30])
plt.legend(['state feedback', 'output feedback', 'reference'])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Lateral position $y/b$')
ax = plt.subplot(2, 2, 2)
plt.plot(t, x[1])
plt.plot(t, x[3])
plt.plot(t, theta_ref, 'k-.')
ax.set(xlim=[0, 15])
plt.ylabel('Vehicle angle $\\theta$')
ax = plt.subplot(2, 2, 4)
plt.plot(t, u_sfb[0])
plt.plot(t, u_ofb[0])
plt.plot(t, delta_curvy, 'k-.')
ax.set(xlim=[0, 15])
plt.xlabel('Normalized time $v_0 t / b$')
plt.ylabel('Steering angle $\\delta$')
plt.tight_layout()
KJA comment, 1 Jul 2019:
In summary I think it is OK to change the reference trajectories but we should make sure that the curvature is less than $\rho=600 m$ not to have too high acceleratarion.
RMM response, 16 Jul 2019:
KJA response, 20 Jul 2019: You are right the curvature is not too small. We could add the sentence "The small deviations can be eliminated by adding feedback."
RMM response, 23 Jul 2019: I think the small deviation you are referring to is in the velocity trace. This occurs because I gave a fixed endpoint in time and so the velocity had to be adjusted to hit that exact point at that time. This doesn't show up in the book, so it won't be a problem ($\implies$ no additional explanation required).
In [9]:
import control.flatsys as fs
# Function to take states, inputs and return the flat flag
def vehicle_flat_forward(x, u, params={}):
# Get the parameter values
b = params.get('wheelbase', 3.)
# Create a list of arrays to store the flat output and its derivatives
zflag = [np.zeros(3), np.zeros(3)]
# Flat output is the x, y position of the rear wheels
zflag[0][0] = x[0]
zflag[1][0] = x[1]
# First derivatives of the flat output
zflag[0][1] = u[0] * np.cos(x[2]) # dx/dt
zflag[1][1] = u[0] * np.sin(x[2]) # dy/dt
# First derivative of the angle
thdot = (u[0]/b) * np.tan(u[1])
# Second derivatives of the flat output (setting vdot = 0)
zflag[0][2] = -u[0] * thdot * np.sin(x[2])
zflag[1][2] = u[0] * thdot * np.cos(x[2])
return zflag
# Function to take the flat flag and return states, inputs
def vehicle_flat_reverse(zflag, params={}):
# Get the parameter values
b = params.get('wheelbase', 3.)
# Create a vector to store the state and inputs
x = np.zeros(3)
u = np.zeros(2)
# Given the flat variables, solve for the state
x[0] = zflag[0][0] # x position
x[1] = zflag[1][0] # y position
x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # tan(theta) = ydot/xdot
# And next solve for the inputs
u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
u[1] = np.arctan2(thdot_v, u[0]**2 / b)
return x, u
vehicle_flat = fs.FlatSystem(vehicle_flat_forward, vehicle_flat_reverse, inputs=2, states=3)
To find a trajectory from an initial state $x_0$ to a final state $x_\text{f}$ in time $T_\text{f}$ we solve a point-to-point trajectory generation problem. We also set the initial and final inputs, which sets the vehicle velocity $v$ and steering wheel angle $\delta$ at the endpoints.
In [10]:
# Define the endpoints of the trajectory
x0 = [0., 2., 0.]; u0 = [15, 0.]
xf = [75, -2., 0.]; uf = [15, 0.]
Tf = xf[0] / uf[0]
# Define a set of basis functions to use for the trajectories
poly = fs.PolyFamily(6)
# Find a trajectory between the initial condition and the final condition
traj = fs.point_to_point(vehicle_flat, x0, u0, xf, uf, Tf, basis=poly)
# Create the trajectory
t = np.linspace(0, Tf, 100)
x, u = traj.eval(t)
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure(figsize=[9, 4.5])
# Plot the trajectory in xy coordinate
plt.subplot(1, 4, 2)
plt.plot(x[1], x[0])
plt.xlabel('y [m]')
plt.ylabel('x [m]')
# Add lane lines and scale the axis
plt.plot([-4, -4], [0, x[0, -1]], 'k-', linewidth=1)
plt.plot([0, 0], [0, x[0, -1]], 'k--', linewidth=1)
plt.plot([4, 4], [0, x[0, -1]], 'k-', linewidth=1)
plt.axis([-10, 10, -5, x[0, -1] + 5])
# Time traces of the state and input
plt.subplot(2, 4, 3)
plt.plot(t, x[1])
plt.ylabel('y [m]')
plt.subplot(2, 4, 4)
plt.plot(t, x[2])
plt.ylabel('theta [rad]')
plt.subplot(2, 4, 7)
plt.plot(t, u[0])
plt.xlabel('Time t [sec]')
plt.ylabel('v [m/s]')
plt.axis([0, Tf, u0[0] - 1, uf[0] +1])
plt.subplot(2, 4, 8)
plt.plot(t, u[1]);
plt.xlabel('Time t [sec]')
plt.ylabel('$\delta$ [rad]')
plt.tight_layout()
The vehicle steering model has different properties depending on whether we are driving forward or in reverse. The figures below show step responses from steering angle to lateral translation for a the linearized model when driving forward (dashed) and reverse (solid). In this simulation we have added an extra pole with the time constant $T=0.1$ to approximately account for the dynamics in the steering system.
With rear-wheel steering the center of mass first moves in the wrong direction and the overall response with rear-wheel steering is significantly delayed compared with that for front-wheel steering. (b) Frequency response for driving forward (dashed) and reverse (solid). Notice that the gain curves are identical, but the phase curve for driving in reverse has non-minimum phase.
RMM note, 27 Jun 2019:
KJA comment, 1 Jul 2019: The reason why you cannot recreate figures i Example 10.11 is because the caption in figure is wrong, sorry my fault, the y-axis should be lateral position not lateral velocity. The approximate expression for the transfer functions
$$ G_{y\delta}=\frac{av_0s+v_0^2}{bs} = \frac{1.5 s + 1}{3s^2}=\frac{0.5s + 0.33}{s} $$are quite close to the values that you get numerically
In this case I think it is useful to have v=1 m/s because we do not drive to fast backwards.
RMM response, 17 Jul 2019
KJA comment, 20 Jul 2019: I have been oscillating a bit about this example. Of course it does not make sense to drive in reverse in 30 m/s but it seems a bit silly to change parameters just in this case (if we do we have to motivate it). On the other hand what we are doing is essentially based on transfer functions and a RHP zero. My current view which has changed a few times is to keep the standard parameters. In any case we should eliminate the extra time constant. A small detail, I could not see the time response in the file you sent, do not resend it!, I will look at the final version.
RMM comment, 23 Jul 2019: I think it is OK to have the speed be different and just talk about this in the text. I have removed the extra time constant in the current version.
In [11]:
# Magnitude of the steering input (half maximum)
Msteer = vehicle_params['maxsteer'] / 2
# Create a linearized model of the system going forward at 2 m/s
forward_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': 2})
forward_tf = ct.ss2tf(forward_lateral)[0, 0]
print("Forward TF = ", forward_tf)
# Create a linearized model of the system going in reverise at 1 m/s
reverse_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': -2})
reverse_tf = ct.ss2tf(reverse_lateral)[0, 0]
print("Reverse TF = ", reverse_tf)
In [12]:
# Configure matplotlib plots to be a bit bigger and optimize layout
plt.figure()
# Forward motion
t, y = ct.step_response(forward_tf * Msteer, np.linspace(0, 4, 500))
plt.plot(t, y, 'b--')
# Reverse motion
t, y = ct.step_response(reverse_tf * Msteer, np.linspace(0, 4, 500))
plt.plot(t, y, 'b-')
# Add labels and reference lines
plt.axis([0, 4, -0.5, 2.5])
plt.legend(['forward', 'reverse'], loc='upper left')
plt.xlabel('Time $t$ [s]')
plt.ylabel('Lateral position [m]')
plt.plot([0, 4], [0, 0], 'k-', linewidth=1)
# Plot the Bode plots
plt.figure()
plt.subplot(1, 2, 2)
ct.bode_plot(forward_tf[0, 0], np.logspace(-1, 1, 100), color='b', linestyle='--')
ct.bode_plot(reverse_tf[0, 0], np.logspace(-1, 1, 100), color='b', linestyle='-')
plt.legend(('forward', 'reverse'));
For a lane transfer system we would like to have a nice response without overshoot, and we therefore consider the use of feedforward compensation to provide a reference trajectory for the closed loop system. We choose the desired response as $F_\text{m}(s) = a^22/(s + a)^2$, where the response speed or aggressiveness of the steering is governed by the parameter $a$.
RMM note, 27 Jun 2019:
KJA comments, 1 Jul 2019: Chosen parameters look good to me
RMM response, 17 Jul 2019
KJA response, 20 Jul 2019: I think the time for lane change is too long, which may depend on the small steering angles used. The largest steering angle is about 0.03 rad, but we have admitted larger values in previous examples. I suggest that we change the design so that the largest sterring angel is closer to 0.05, see the remark from Bjorn O a lane change could take about 5 s at 30m/s.
RMM response, 23 Jul 2019: I reset the time constant to 0.2, which gives something closer to what we had for trajectory generation. It is still slower, but this is to be expected since it is a linear controller. We now finish the trajectory in 20 body lengths, which is about 6 seconds.
In [13]:
# Define the desired response of the system
a = 0.2
P = ct.ss2tf(lateral_normalized)
Fm = ct.TransferFunction([a**2], [1, 2*a, a**2])
Fr = Fm / P
# Compute the step response of the feedforward components
t, y_ffwd = ct.step_response(Fm, np.linspace(0, 25, 100))
t, delta_ffwd = ct.step_response(Fr, np.linspace(0, 25, 100))
# Scale and shift to correspond to lane change (-2 to +2)
y_ffwd = 0.5 - 1 * y_ffwd
delta_ffwd *= 1
# Overhead view
plt.subplot(1, 2, 1)
plt.plot(y_ffwd, t)
plt.plot(-1*np.ones(t.shape), t, 'k-', linewidth=1)
plt.plot(0*np.ones(t.shape), t, 'k--', linewidth=1)
plt.plot(1*np.ones(t.shape), t, 'k-', linewidth=1)
plt.axis([-5, 5, -2, 27])
# Plot the response
plt.subplot(2, 2, 2)
plt.plot(t, y_ffwd)
# plt.axis([0, 10, -5, 5])
plt.ylabel('Normalized position y/b')
plt.subplot(2, 2, 4)
plt.plot(t, delta_ffwd)
# plt.axis([0, 10, -1, 1])
plt.ylabel('$\\delta$ [rad]')
plt.xlabel('Normalized time $v_0 t / b$');
plt.tight_layout()
KJA comment, 20 Jul 2019: This is a really troublesome case. If we keep it as a vehicle steering problem we must have an order of magnitude lower valuer for $\omega_c$ and $\omega_o$ and then the zero will not be slow. My recommendation is to keep it as a general system with the transfer function. $P(s)=(s+1)/s^2$. The text then has to be reworded.
RMM response, 23 Jul 2019: I think the way we have it is OK. Our current value for the controller and observer is $\omega_\text{c} = 0.7$ and $\omega_\text{o} = 1$. Here we way we want something faster and so we got to $\omega_\text{c} = 7$ (10X) and $\omega_\text{o} = 10$ (10X).
In [14]:
# Compute the feedback gain using eigenvalue placement
wc = 10
zc = 0.707
eigs = np.roots([1, 2*zc*wc, wc**2])
K = ct.place(A, B, eigs)
kr = np.real(1/clsys.evalfr(0))
print("K = ", np.squeeze(K))
# Compute the estimator gain using eigenvalue placement
wo = 20
zo = 0.707
eigs = np.roots([1, 2*zo*wo, wo**2])
L = np.transpose(
ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", np.squeeze(L))
# Construct an output-based controller for the system
C1 = ct.ss2tf(ct.StateSpace(A - B@K - L@C, L, K, 0))
print("C(s) = ", C1)
# Compute the loop transfer function and plot Nyquist, Bode
L1 = P * C1
plt.figure(); ct.nyquist_plot(L1, np.logspace(0.5, 3, 500))
plt.figure(); ct.bode_plot(L1, np.logspace(-1, 3, 500));
In [15]:
# Modified control law
wc = 10
zc = 2.6
eigs = np.roots([1, 2*zc*wc, wc**2])
K = ct.place(A, B, eigs)
kr = np.real(1/clsys.evalfr(0))
print("K = ", np.squeeze(K))
# Construct an output-based controller for the system
C2 = ct.ss2tf(ct.StateSpace(A - B@K - L@C, L, K, 0))
print("C(s) = ", C2)
In [16]:
# Plot the gang of four for the two designs
ct.gangof4(P, C1, np.logspace(-1, 3, 100))
ct.gangof4(P, C2, np.logspace(-1, 3, 100))
In [ ]: