velocity as mpc input
matssteinweg
parent
d8d596bb14
commit
7e7ff06029
84
MPC.py
84
MPC.py
|
|
@ -13,7 +13,8 @@ PREDICTION = '#BA4A00'
|
|||
|
||||
|
||||
class MPC:
|
||||
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
|
||||
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints,
|
||||
velocity_reference):
|
||||
"""
|
||||
Constructor for the Model Predictive Controller.
|
||||
:param model: bicycle model object to be controlled
|
||||
|
|
@ -23,6 +24,7 @@ class MPC:
|
|||
:param QN: final state cost matrix
|
||||
:param StateConstraints: dictionary of state constraints
|
||||
:param InputConstraints: dictionary of input constraints
|
||||
:param velocity_reference: reference value for velocity
|
||||
"""
|
||||
|
||||
# Parameters
|
||||
|
|
@ -34,10 +36,17 @@ class MPC:
|
|||
# Model
|
||||
self.model = model
|
||||
|
||||
# Dimensions
|
||||
self.nx = self.model.n_states
|
||||
self.nu = 2
|
||||
|
||||
# Constraints
|
||||
self.state_constraints = StateConstraints
|
||||
self.input_constraints = InputConstraints
|
||||
|
||||
# Velocity reference
|
||||
self.v_ref = velocity_reference
|
||||
|
||||
# Current control and prediction
|
||||
self.current_prediction = None
|
||||
|
||||
|
|
@ -45,20 +54,16 @@ class MPC:
|
|||
self.infeasibility_counter = 0
|
||||
|
||||
# Current control signals
|
||||
self.current_control = None
|
||||
self.current_control = np.ones((self.nu*self.N)) * velocity_reference
|
||||
|
||||
# Initialize Optimization Problem
|
||||
self.optimizer = osqp.OSQP()
|
||||
|
||||
def _init_problem(self, v):
|
||||
def _init_problem(self):
|
||||
"""
|
||||
Initialize optimization problem for current time step.
|
||||
"""
|
||||
|
||||
# Number of state and input variables
|
||||
nx = self.model.n_states
|
||||
nu = 1
|
||||
|
||||
# Constraints
|
||||
umin = self.input_constraints['umin']
|
||||
umax = self.input_constraints['umax']
|
||||
|
|
@ -66,14 +71,13 @@ class MPC:
|
|||
xmax = self.state_constraints['xmax']
|
||||
|
||||
# LTV System Matrices
|
||||
A = np.zeros((nx * (self.N + 1), nx * (self.N + 1)))
|
||||
B = np.zeros((nx * (self.N + 1), nu * (self.N)))
|
||||
A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1)))
|
||||
B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
|
||||
# Reference vector for state and input variables
|
||||
ur = np.zeros(self.N)
|
||||
xr = np.array([0.0, 0.0, -1.0])
|
||||
ur = np.zeros(self.nu*self.N)
|
||||
xr = np.array([0.0, 0.0, 0.0])
|
||||
# Offset for equality constraint (due to B * (u - ur))
|
||||
uq = np.zeros(self.N * nx)
|
||||
|
||||
uq = np.zeros(self.N * self.nx)
|
||||
# Dynamic state constraints
|
||||
xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
|
||||
xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
|
||||
|
|
@ -82,34 +86,34 @@ class MPC:
|
|||
for n in range(self.N):
|
||||
|
||||
# Get information about current waypoint
|
||||
current_waypoint = self.model.reference_path.waypoints[
|
||||
self.model.wp_id + n]
|
||||
next_waypoint = self.model.reference_path.waypoints[
|
||||
self.model.wp_id + n + 1]
|
||||
current_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n)
|
||||
next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1)
|
||||
delta_s = next_waypoint - current_waypoint
|
||||
kappa_r = current_waypoint.kappa
|
||||
kappa_ref = current_waypoint.kappa
|
||||
|
||||
# Compute LTV matrices
|
||||
f, A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
|
||||
A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin
|
||||
B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin
|
||||
f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s)
|
||||
A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin
|
||||
B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin
|
||||
|
||||
# Set kappa_r to reference for input signal
|
||||
ur[n] = kappa_r
|
||||
# Set reference for input signal
|
||||
ur[n*self.nu:(n+1)*self.nu] = np.array([self.v_ref, kappa_ref])
|
||||
# Compute equality constraint offset (B*ur)
|
||||
uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - f
|
||||
uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array
|
||||
([self.v_ref, kappa_ref])) - f
|
||||
# Compute dynamic constraints on e_y
|
||||
lb, ub = self.model.reference_path.update_bounds(
|
||||
self.model.wp_id + n, self.model.safety_margin[1])
|
||||
xmin_dyn[nx * n] = lb
|
||||
xmax_dyn[nx * n] = ub
|
||||
xmin_dyn[self.nx * n] = lb
|
||||
xmax_dyn[self.nx * n] = ub
|
||||
|
||||
# Get equality matrix
|
||||
Ax = sparse.kron(sparse.eye(self.N + 1),
|
||||
-sparse.eye(nx)) + sparse.csc_matrix(A)
|
||||
-sparse.eye(self.nx)) + sparse.csc_matrix(A)
|
||||
Bu = sparse.csc_matrix(B)
|
||||
Aeq = sparse.hstack([Ax, Bu])
|
||||
# Get inequality matrix
|
||||
Aineq = sparse.eye((self.N + 1) * nx + self.N * nu)
|
||||
Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu)
|
||||
# Combine constraint matrices
|
||||
A = sparse.vstack([Aeq, Aineq], format='csc')
|
||||
|
||||
|
|
@ -131,13 +135,13 @@ class MPC:
|
|||
sparse.kron(sparse.eye(self.N), self.R)], format='csc')
|
||||
q = np.hstack(
|
||||
[np.kron(np.ones(self.N), -self.Q.dot(xr)), -self.QN.dot(xr),
|
||||
-self.R.A[0, 0] * ur])
|
||||
-np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur])
|
||||
|
||||
# Initialize optimizer
|
||||
self.optimizer = osqp.OSQP()
|
||||
self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
|
||||
|
||||
def get_control(self, v):
|
||||
def get_control(self):
|
||||
"""
|
||||
Get control signal given the current position of the car. Solves a
|
||||
finite time optimization problem based on the linearized car model.
|
||||
|
|
@ -145,6 +149,7 @@ class MPC:
|
|||
|
||||
# Number of state variables
|
||||
nx = self.model.n_states
|
||||
nu = 2
|
||||
|
||||
# Update current waypoint
|
||||
self.model.get_current_waypoint()
|
||||
|
|
@ -153,21 +158,24 @@ class MPC:
|
|||
self.model.spatial_state = self.model.t2s()
|
||||
|
||||
# Initialize optimization problem
|
||||
self._init_problem(v)
|
||||
self._init_problem()
|
||||
|
||||
# Solve optimization problem
|
||||
dec = self.optimizer.solve()
|
||||
|
||||
try:
|
||||
# Get control signals
|
||||
deltas = np.arctan(dec.x[-self.N:] * self.model.l)
|
||||
delta = deltas[0]
|
||||
control_signals = np.array(dec.x[-self.N*nu:])
|
||||
control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.l)
|
||||
v = control_signals[0]
|
||||
delta = control_signals[1]
|
||||
|
||||
# Update control signals
|
||||
self.current_control = deltas
|
||||
self.current_control = control_signals
|
||||
|
||||
# Get predicted spatial states
|
||||
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
|
||||
|
||||
# Update predicted temporal states
|
||||
self.current_prediction = self.update_prediction(delta, x)
|
||||
|
||||
|
|
@ -181,8 +189,8 @@ class MPC:
|
|||
|
||||
print('Infeasible problem. Previously predicted'
|
||||
' control signal used!')
|
||||
u = np.array([v, self.current_control
|
||||
[self.infeasibility_counter+1]])
|
||||
id = nu * (self.infeasibility_counter + 1)
|
||||
u = np.array(self.current_control[id:id+2])
|
||||
|
||||
# increase infeasibility counter
|
||||
self.infeasibility_counter += 1
|
||||
|
|
@ -208,10 +216,10 @@ class MPC:
|
|||
#print('#########################')
|
||||
|
||||
for n in range(2, self.N):
|
||||
associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
|
||||
associated_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id+n)
|
||||
predicted_temporal_state = self.model.s2t(associated_waypoint,
|
||||
spatial_state_prediction[n, :])
|
||||
print(spatial_state_prediction[n, 2])
|
||||
#print(spatial_state_prediction[n, 2])
|
||||
#print('delta: ', u)
|
||||
#print('e_y: ', spatial_state_prediction[n, 0])
|
||||
#print('e_psi: ', spatial_state_prediction[n, 1])
|
||||
|
|
|
|||
|
|
@ -84,7 +84,7 @@ class Obstacle:
|
|||
|
||||
class ReferencePath:
|
||||
def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance,
|
||||
max_width, n_extension, circular):
|
||||
max_width, circular):
|
||||
"""
|
||||
Reference Path object. Create a reference trajectory from specified
|
||||
corner points with given resolution. Smoothing around corners can be
|
||||
|
|
@ -97,8 +97,6 @@ class ReferencePath:
|
|||
:param smoothing_distance: number of waypoints used for smoothing the
|
||||
path by averaging neighborhood of waypoints
|
||||
:param max_width: maximum width of path to both sides in m
|
||||
:param n_extension: number of samples used for path extension to allow
|
||||
for MPC
|
||||
:param circular: True if path circular
|
||||
"""
|
||||
|
||||
|
|
@ -114,15 +112,15 @@ class ReferencePath:
|
|||
# Look ahead distance for path averaging
|
||||
self.smoothing_distance = smoothing_distance
|
||||
|
||||
# Number of waypoints used to extend path at the end
|
||||
self.n_extension = n_extension
|
||||
|
||||
# Circular
|
||||
self.circular = circular
|
||||
|
||||
# List of waypoint objects
|
||||
self.waypoints = self._construct_path(wp_x, wp_y)
|
||||
|
||||
# Number of waypoints
|
||||
self.n_waypoints = len(self.waypoints)
|
||||
|
||||
# Length of path
|
||||
self.length, self.segment_lengths = self._compute_length()
|
||||
|
||||
|
|
@ -154,14 +152,6 @@ class ReferencePath:
|
|||
tolist() for i in range(len(wp_y) - 1)]
|
||||
wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
|
||||
|
||||
if self.n_extension is not None:
|
||||
if self.circular:
|
||||
wp_x += wp_x[:self.n_extension]
|
||||
wp_y += wp_y[:self.n_extension]
|
||||
else:
|
||||
wp_x += wp_x[-self.n_extension:]
|
||||
wp_y += wp_y[-self.n_extension:]
|
||||
|
||||
# Smooth path
|
||||
wp_xs = []
|
||||
wp_ys = []
|
||||
|
|
@ -232,7 +222,7 @@ class ReferencePath:
|
|||
:return: length of center-line path in m
|
||||
"""
|
||||
segment_lengths = [0.0] + [self.waypoints[wp_id+1] - self.waypoints
|
||||
[wp_id] for wp_id in range(len(self.waypoints)-self.n_extension-1)]
|
||||
[wp_id] for wp_id in range(len(self.waypoints)-1)]
|
||||
s = sum(segment_lengths)
|
||||
return s, segment_lengths
|
||||
|
||||
|
|
@ -327,7 +317,7 @@ class ReferencePath:
|
|||
"""
|
||||
|
||||
# Get reference waypoint
|
||||
wp = self.waypoints[wp_id]
|
||||
wp = self.get_waypoint(wp_id)
|
||||
|
||||
# Get waypoint's border cells in map coordinates
|
||||
ub_p = self.map.w2m(wp.border_cells[0][0], wp.border_cells[0][1])
|
||||
|
|
@ -394,11 +384,6 @@ class ReferencePath:
|
|||
ub_ls = wp.x + ub * np.cos(angle_ub), wp.y + ub * np.sin(angle_ub)
|
||||
lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(angle_lb)
|
||||
|
||||
# Update member variables of waypoint
|
||||
#wp.ub = ub
|
||||
#wp.lb = lb
|
||||
#wp.border_cells = (ub_ls, lb_ls)
|
||||
|
||||
return lb, ub
|
||||
|
||||
def add_obstacles(self, obstacles):
|
||||
|
|
@ -444,14 +429,14 @@ class ReferencePath:
|
|||
vmax=1.0)
|
||||
|
||||
# Get x and y coordinates for all waypoints
|
||||
wp_x = np.array([wp.x for wp in self.waypoints][:-self.n_extension])
|
||||
wp_y = np.array([wp.y for wp in self.waypoints][:-self.n_extension])
|
||||
wp_x = np.array([wp.x for wp in self.waypoints])
|
||||
wp_y = np.array([wp.y for wp in self.waypoints])
|
||||
|
||||
# Get x and y locations of border cells for upper and lower bound
|
||||
wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints][:-self.n_extension])
|
||||
wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension])
|
||||
wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension])
|
||||
wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints][:-self.n_extension])
|
||||
wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints])
|
||||
wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints])
|
||||
wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints])
|
||||
wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints])
|
||||
|
||||
# Plot waypoints
|
||||
plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3)
|
||||
|
|
@ -467,16 +452,16 @@ class ReferencePath:
|
|||
|
||||
# Plot border of path
|
||||
bl_x = np.array([wp.border_cells[0][0] for wp in
|
||||
self.waypoints][:-self.n_extension] +
|
||||
self.waypoints] +
|
||||
[self.waypoints[0].border_cells[0][0]])
|
||||
bl_y = np.array([wp.border_cells[0][1] for wp in
|
||||
self.waypoints][:-self.n_extension] +
|
||||
self.waypoints] +
|
||||
[self.waypoints[0].border_cells[0][1]])
|
||||
br_x = np.array([wp.border_cells[1][0] for wp in
|
||||
self.waypoints][:-self.n_extension] +
|
||||
self.waypoints] +
|
||||
[self.waypoints[0].border_cells[1][0]])
|
||||
br_y = np.array([wp.border_cells[1][1] for wp in
|
||||
self.waypoints][:-self.n_extension] +
|
||||
self.waypoints] +
|
||||
[self.waypoints[0].border_cells[1][1]])
|
||||
# Smooth border
|
||||
# bl_x = savgol_filter(bl_x, 15, 9)
|
||||
|
|
@ -499,11 +484,22 @@ class ReferencePath:
|
|||
for obstacle in self.obstacles:
|
||||
obstacle.show()
|
||||
|
||||
def get_waypoint(self, wp_id):
|
||||
if wp_id >= self.n_waypoints and self.circular:
|
||||
wp_id = np.mod(wp_id, self.n_waypoints)
|
||||
elif wp_id >= self.n_waypoints and not self.circular:
|
||||
print('Reached end of path!')
|
||||
exit(1)
|
||||
|
||||
return self.waypoints[wp_id]
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# Select Path | 'Race' or 'Q'
|
||||
path = 'Race'
|
||||
path = 'Q'
|
||||
|
||||
# Create Map
|
||||
if path == 'Race':
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ if __name__ == '__main__':
|
|||
# Create smoothed reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=0.23,
|
||||
n_extension=50, circular=True)
|
||||
circular=True)
|
||||
elif sim_mode == 'Q':
|
||||
map = Map(file_path='map_floor2.png')
|
||||
wp_x = [-9.169, 11.9, 7.3, -6.95]
|
||||
|
|
@ -37,7 +37,7 @@ if __name__ == '__main__':
|
|||
# Create smoothed reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=1.50,
|
||||
n_extension=50, circular=False)
|
||||
circular=False)
|
||||
else:
|
||||
print('Invalid Simulation Mode!')
|
||||
map, wp_x, wp_y, path_resolution, reference_path \
|
||||
|
|
@ -65,7 +65,6 @@ if __name__ == '__main__':
|
|||
e_y_0 = 0.0
|
||||
e_psi_0 = 0.0
|
||||
t_0 = 0.0
|
||||
v = 1.0
|
||||
|
||||
car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path,
|
||||
e_y=e_y_0, e_psi=e_psi_0, t=t_0)
|
||||
|
|
@ -76,13 +75,15 @@ if __name__ == '__main__':
|
|||
|
||||
N = 30
|
||||
Q = sparse.diags([1.0, 0.0, 0.0])
|
||||
R = sparse.diags([0.01])
|
||||
QN = sparse.diags([0.0, 0.0, 1.0])
|
||||
InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]),
|
||||
'umax': np.array([np.tan(0.66)/car.l])}
|
||||
R = sparse.diags([1.0, 0.0])
|
||||
QN = sparse.diags([0.0, 0.0, 0.0])
|
||||
InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]),
|
||||
'umax': np.array([2.5, np.tan(0.66)/car.l])}
|
||||
StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),
|
||||
'xmax': np.array([np.inf, np.inf, np.inf])}
|
||||
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints)
|
||||
velocity_reference = 1.5 # m/s
|
||||
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints,
|
||||
velocity_reference)
|
||||
|
||||
#########
|
||||
# LiDAR #
|
||||
|
|
@ -107,17 +108,11 @@ if __name__ == '__main__':
|
|||
while car.s < reference_path.length:
|
||||
|
||||
# get control signals
|
||||
start = time()
|
||||
u = mpc.get_control(v)
|
||||
end = time()
|
||||
print('Control time: ', end-start)
|
||||
u = mpc.get_control()
|
||||
|
||||
# drive car
|
||||
car.drive(u)
|
||||
|
||||
# scan
|
||||
scan = sensor.scan(car.temporal_state, map)
|
||||
|
||||
# log
|
||||
x_log.append(car.temporal_state.x)
|
||||
y_log.append(car.temporal_state.y)
|
||||
|
|
@ -129,9 +124,6 @@ if __name__ == '__main__':
|
|||
# Plot path and drivable area
|
||||
reference_path.show()
|
||||
|
||||
# Plot scan
|
||||
sensor.plot_scan(car.temporal_state)
|
||||
|
||||
# Plot MPC prediction
|
||||
mpc.show_prediction()
|
||||
|
||||
|
|
@ -140,11 +132,10 @@ if __name__ == '__main__':
|
|||
|
||||
t += Ts
|
||||
|
||||
plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: '
|
||||
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
|
||||
'{:.2f} s'.
|
||||
format(car.s, car.reference_path.length, t))
|
||||
if t == Ts:
|
||||
plt.show()
|
||||
plt.pause(0.0001)
|
||||
format(u[0], u[1], t))
|
||||
|
||||
plt.pause(0.01)
|
||||
print('Final Time: {:.2f}'.format(t))
|
||||
plt.close()
|
||||
|
|
|
|||
|
|
@ -436,9 +436,9 @@ class BicycleModel(SpatialBicycleModel):
|
|||
a_2 = np.array([-kappa_r**2*delta_s, 1, 0])
|
||||
a_3 = np.array([-kappa_r/v*delta_s, 0, 1])
|
||||
|
||||
b_1 = np.array([0, ])
|
||||
b_2 = np.array([delta_s, ])
|
||||
b_3 = np.array([0, ])
|
||||
b_1 = np.array([0, 0])
|
||||
b_2 = np.array([0, delta_s])
|
||||
b_3 = np.array([-1/(v**2)*delta_s, 0])
|
||||
|
||||
f = np.array([0.0, 0.0, 1/v*delta_s])
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue