Tidy up a bit. Add comments.

master
matssteinweg 2020-01-02 17:10:14 +01:00
parent 281fc19b6d
commit 58a9a112a4
5 changed files with 195 additions and 155 deletions

59
MPC.py
View File

@ -2,7 +2,6 @@ import numpy as np
import osqp import osqp
from scipy import sparse from scipy import sparse
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from time import time
# Colors # Colors
PREDICTION = '#BA4A00' PREDICTION = '#BA4A00'
@ -13,16 +12,18 @@ PREDICTION = '#BA4A00'
class MPC: class MPC:
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints,
ay_max):
""" """
Constructor for the Model Predictive Controller. Constructor for the Model Predictive Controller.
:param model: bicycle model object to be controlled :param model: bicycle model object to be controlled
:param T: time horizon | int :param N: time horizon | int
:param Q: state cost matrix :param Q: state cost matrix
:param R: input cost matrix :param R: input cost matrix
:param QN: final state cost matrix :param QN: final state cost matrix
:param StateConstraints: dictionary of state constraints :param StateConstraints: dictionary of state constraints
:param InputConstraints: dictionary of input constraints :param InputConstraints: dictionary of input constraints
:param ay_max: maximum allowed lateral acceleration in curves
""" """
# Parameters # Parameters
@ -43,7 +44,7 @@ class MPC:
self.input_constraints = InputConstraints self.input_constraints = InputConstraints
# Maximum lateral acceleration # Maximum lateral acceleration
self.ay_max = 5.0 self.ay_max = ay_max
# Current control and prediction # Current control and prediction
self.current_prediction = None self.current_prediction = None
@ -52,7 +53,7 @@ class MPC:
self.infeasibility_counter = 0 self.infeasibility_counter = 0
# Current control signals # Current control signals
self.current_control = np.ones((self.nu*self.N)) self.current_control = np.zeros((self.nu*self.N))
# Initialize Optimization Problem # Initialize Optimization Problem
self.optimizer = osqp.OSQP() self.optimizer = osqp.OSQP()
@ -73,7 +74,7 @@ class MPC:
B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
# Reference vector for state and input variables # Reference vector for state and input variables
ur = np.zeros(self.nu*self.N) ur = np.zeros(self.nu*self.N)
xr = np.array([0.0, 0.0, 0.0]) xr = np.zeros(self.nx*(self.N+1))
# Offset for equality constraint (due to B * (u - ur)) # Offset for equality constraint (due to B * (u - ur))
uq = np.zeros(self.N * self.nx) uq = np.zeros(self.N * self.nx)
# Dynamic state constraints # Dynamic state constraints
@ -83,7 +84,7 @@ class MPC:
umax_dyn = np.kron(np.ones(self.N), umax) umax_dyn = np.kron(np.ones(self.N), umax)
# Get curvature predictions from previous control signals # Get curvature predictions from previous control signals
kappa_pred = np.tan(np.array(self.current_control[3::] + kappa_pred = np.tan(np.array(self.current_control[3::] +
self.current_control[-1:])) / self.model.l self.current_control[-1:])) / self.model.length
# Iterate over horizon # Iterate over horizon
for n in range(self.N): for n in range(self.N):
@ -113,13 +114,16 @@ class MPC:
# Compute dynamic constraints on e_y # Compute dynamic constraints on e_y
ub, lb, _ = self.model.reference_path.update_path_constraints( ub, lb, _ = self.model.reference_path.update_path_constraints(
self.model.wp_id+1, self.N, 2*self.model.safety_margin[1], self.model.wp_id+1, self.N, 2*self.model.safety_margin,
self.model.safety_margin[1]) self.model.safety_margin)
xmin_dyn[0] = self.model.spatial_state.e_y xmin_dyn[0] = self.model.spatial_state.e_y
xmax_dyn[0] = self.model.spatial_state.e_y xmax_dyn[0] = self.model.spatial_state.e_y
xmin_dyn[self.nx::self.nx] = lb xmin_dyn[self.nx::self.nx] = lb
xmax_dyn[self.nx::self.nx] = ub xmax_dyn[self.nx::self.nx] = ub
# Set reference for state as center-line of drivable area
xr[self.nx::self.nx] = (lb + ub) / 2
# Get equality matrix # Get equality matrix
Ax = sparse.kron(sparse.eye(self.N + 1), Ax = sparse.kron(sparse.eye(self.N + 1),
-sparse.eye(self.nx)) + sparse.csc_matrix(A) -sparse.eye(self.nx)) + sparse.csc_matrix(A)
@ -146,8 +150,9 @@ class MPC:
P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN, P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN,
sparse.kron(sparse.eye(self.N), self.R)], format='csc') sparse.kron(sparse.eye(self.N), self.R)], format='csc')
q = np.hstack( q = np.hstack(
[np.kron(np.ones(self.N), -self.Q.dot(xr)), -self.QN.dot(xr), [-np.tile(np.diag(self.Q.A), self.N) * xr[:-self.nx],
-np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur]) -self.QN.dot(xr[-self.nx:]),
-np.tile(np.diag(self.R.A), self.N) * ur])
# Initialize optimizer # Initialize optimizer
self.optimizer = osqp.OSQP() self.optimizer = osqp.OSQP()
@ -167,7 +172,9 @@ class MPC:
self.model.get_current_waypoint() self.model.get_current_waypoint()
# Update spatial state # Update spatial state
self.model.spatial_state = self.model.t2s() self.model.spatial_state = self.model.t2s(reference_state=
self.model.temporal_state, reference_waypoint=
self.model.current_waypoint)
# Initialize optimization problem # Initialize optimization problem
self._init_problem() self._init_problem()
@ -178,7 +185,8 @@ class MPC:
try: try:
# Get control signals # Get control signals
control_signals = np.array(dec.x[-self.N*nu:]) control_signals = np.array(dec.x[-self.N*nu:])
control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.l) control_signals[1::2] = np.arctan(control_signals[1::2] *
self.model.length)
v = control_signals[0] v = control_signals[0]
delta = control_signals[1] delta = control_signals[1]
@ -189,7 +197,7 @@ class MPC:
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
# Update predicted temporal states # Update predicted temporal states
self.current_prediction = self.update_prediction(delta, x) self.current_prediction = self.update_prediction(x)
# Get current control signal # Get current control signal
u = np.array([v, delta]) u = np.array([v, delta])
@ -213,7 +221,7 @@ class MPC:
return u return u
def update_prediction(self, u, spatial_state_prediction): def update_prediction(self, spatial_state_prediction):
""" """
Transform the predicted states to predicted x and y coordinates. Transform the predicted states to predicted x and y coordinates.
Mainly for visualization purposes. Mainly for visualization purposes.
@ -221,23 +229,19 @@ class MPC:
:return: lists of predicted x and y coordinates :return: lists of predicted x and y coordinates
""" """
# containers for x and y coordinates of predicted states # Containers for x and y coordinates of predicted states
x_pred, y_pred = [], [] x_pred, y_pred = [], []
# get current waypoint ID # Iterate over prediction horizon
#print('#########################')
for n in range(2, self.N): for n in range(2, self.N):
associated_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id+n) # Get associated waypoint
associated_waypoint = self.model.reference_path.\
get_waypoint(self.model.wp_id+n)
# Transform predicted spatial state to temporal state
predicted_temporal_state = self.model.s2t(associated_waypoint, predicted_temporal_state = self.model.s2t(associated_waypoint,
spatial_state_prediction[n, :]) spatial_state_prediction[n, :])
#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])
#print('t: ', spatial_state_prediction[n, 2])
#print('+++++++++++++++++++++++')
# Save predicted coordinates in world coordinate frame
x_pred.append(predicted_temporal_state.x) x_pred.append(predicted_temporal_state.x)
y_pred.append(predicted_temporal_state.y) y_pred.append(predicted_temporal_state.y)
@ -248,6 +252,7 @@ class MPC:
Display predicted car trajectory in current axis. Display predicted car trajectory in current axis.
""" """
plt.scatter(self.current_prediction[0], self.current_prediction[1], if self.current_prediction is not None:
plt.scatter(self.current_prediction[0], self.current_prediction[1],
c=PREDICTION, s=30) c=PREDICTION, s=30)

View File

@ -132,7 +132,7 @@ class LidarModel:
if __name__ == '__main__': if __name__ == '__main__':
# Create Map # Create Map
map = Map('map_floor2.png') map = Map('real_map.png')
plt.imshow(map.data, cmap='gray', plt.imshow(map.data, cmap='gray',
extent=[map.origin[0], map.origin[0] + extent=[map.origin[0], map.origin[0] +
map.width * map.resolution, map.width * map.resolution,

70
map.py
View File

@ -3,11 +3,46 @@ import matplotlib.pyplot as plt
from skimage.morphology import remove_small_holes from skimage.morphology import remove_small_holes
from PIL import Image from PIL import Image
from skimage.draw import line_aa from skimage.draw import line_aa
import matplotlib.patches as plt_patches
# Colors
OBSTACLE = '#2E4053'
############
# Obstacle #
############
class Obstacle:
def __init__(self, cx, cy, radius):
"""
Constructor for a circular obstacle to be placed on a map.
:param cx: x coordinate of center of obstacle in world coordinates
:param cy: y coordinate of center of obstacle in world coordinates
:param radius: radius of circular obstacle in m
"""
self.cx = cx
self.cy = cy
self.radius = radius
def show(self):
"""
Display obstacle on current axis.
"""
# Draw circle
circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
self.radius, color=OBSTACLE, zorder=20)
ax = plt.gca()
ax.add_patch(circle)
#######
# Map #
#######
class Map: class Map:
def __init__(self, file_path, threshold_occupied=100, def __init__(self, file_path, origin, resolution, threshold_occupied=100):
origin=(-30.0, -24.0), resolution=0.06):
""" """
Constructor for map object. Map contains occupancy grid map data of Constructor for map object. Map contains occupancy grid map data of
environment as well as meta information. environment as well as meta information.
@ -65,6 +100,19 @@ class Map:
return x, y return x, y
def process_map(self):
"""
Process raw map image. Binarization and removal of small holes in map.
"""
# Binarization using specified threshold
# 1 corresponds to free, 0 to occupied
self.data = np.where(self.data >= self.threshold_occupied, 1, 0)
# Remove small holes in map corresponding to spurious measurements
self.data = remove_small_holes(self.data, area_threshold=5,
connectivity=8).astype(np.int8)
def add_obstacles(self, obstacles): def add_obstacles(self, obstacles):
""" """
Add obstacles to the map. Add obstacles to the map.
@ -106,21 +154,9 @@ class Map:
for x, y in zip(path_x, path_y): for x, y in zip(path_x, path_y):
self.data[y, x] = 0 self.data[y, x] = 0
def process_map(self):
"""
Process raw map image. Binarization and removal of small holes in map.
"""
# Binarization using specified threshold
# 1 corresponds to free, 0 to occupied
self.data = np.where(self.data >= self.threshold_occupied, 1, 0)
# Remove small holes in map corresponding to spurious measurements
self.data = remove_small_holes(self.data, area_threshold=5,
connectivity=8).astype(np.int8)
if __name__ == '__main__': if __name__ == '__main__':
map = Map('map_floor2.png') map = Map('real_map.png')
plt.imshow(map.data, cmap='gray') # map = Map('sim_map.png')
plt.imshow(np.flipud(map.data), cmap='gray')
plt.show() plt.show()

View File

@ -1,17 +1,17 @@
import numpy as np import numpy as np
import math import math
from map import Map from map import Map, Obstacle
from skimage.draw import line_aa from skimage.draw import line_aa
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import matplotlib.patches as plt_patches
from scipy import sparse from scipy import sparse
import osqp import osqp
# Colors # Colors
DRIVABLE_AREA = '#BDC3C7' DRIVABLE_AREA = '#BDC3C7'
WAYPOINTS = '#D0D3D4' WAYPOINTS = '#D0D3D4'
OBSTACLE = '#2E4053'
PATH_CONSTRAINTS = '#F5B041' PATH_CONSTRAINTS = '#F5B041'
OBSTACLE = '#2E4053'
############ ############
# Waypoint # # Waypoint #
@ -21,7 +21,9 @@ class Waypoint:
def __init__(self, x, y, psi, kappa): def __init__(self, x, y, psi, kappa):
""" """
Waypoint object containing x, y location in global coordinate system, Waypoint object containing x, y location in global coordinate system,
orientation of waypoint psi and local curvature kappa orientation of waypoint psi and local curvature kappa. Waypoint further
contains an associated reference velocity computed by the speed profile
and a path width specified by upper and lower bounds.
:param x: x position in global coordinate system | [m] :param x: x position in global coordinate system | [m]
:param y: y position in global coordinate system | [m] :param y: y position in global coordinate system | [m]
:param psi: orientation of waypoint | [rad] :param psi: orientation of waypoint | [rad]
@ -37,7 +39,9 @@ class Waypoint:
# Information about drivable area at waypoint # Information about drivable area at waypoint
# upper and lower bound of drivable area orthogonal to # upper and lower bound of drivable area orthogonal to
# waypoint orientation # waypoint orientation.
# Upper bound: free drivable area to the left of center-line in m
# Lower bound: free drivable area to the right of center-line in m
self.lb = None self.lb = None
self.ub = None self.ub = None
self.static_border_cells = None self.static_border_cells = None
@ -53,35 +57,6 @@ class Waypoint:
return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5 return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
############
# Obstacle #
############
class Obstacle:
def __init__(self, cx, cy, radius):
"""
Constructor for a circular obstacle to be place on a path.
:param cx: x coordinate of center of obstacle in world coordinates
:param cy: y coordinate of center of obstacle in world coordinates
:param radius: radius of circular obstacle in m
"""
self.cx = cx
self.cy = cy
self.radius = radius
def show(self):
"""
Display obstacle.
"""
# Draw circle
circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
self.radius, color=OBSTACLE, zorder=20)
ax = plt.gca()
ax.add_patch(circle)
################## ##################
# Reference Path # # Reference Path #
################## ##################
@ -155,8 +130,6 @@ class ReferencePath:
wp_y = [wp for segment in wp_y for wp in segment] + [gp_y] wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
# Smooth path # Smooth path
#wp_xs = wp_x[:self.smoothing_distance]
#wp_ys = wp_y[:self.smoothing_distance]
wp_xs = [] wp_xs = []
wp_ys = [] wp_ys = []
for wp_id in range(self.smoothing_distance, len(wp_x) - for wp_id in range(self.smoothing_distance, len(wp_x) -
@ -165,8 +138,6 @@ class ReferencePath:
+ self.smoothing_distance + 1])) + self.smoothing_distance + 1]))
wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
+ self.smoothing_distance + 1])) + self.smoothing_distance + 1]))
#wp_xs += wp_x[-self.smoothing_distance:]
#wp_ys += wp_y[-self.smoothing_distance:]
# Construct list of waypoint objects # Construct list of waypoint objects
waypoints = list(zip(wp_xs, wp_ys)) waypoints = list(zip(wp_xs, wp_ys))
@ -434,7 +405,6 @@ class ReferencePath:
wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints]) wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints])
# Plot waypoints # Plot waypoints
# colors = [wp.v_ref for wp in self.waypoints] # colors = [wp.v_ref for wp in self.waypoints]
plt.scatter(wp_x, wp_y, c=WAYPOINTS, s=10) plt.scatter(wp_x, wp_y, c=WAYPOINTS, s=10)
@ -680,22 +650,28 @@ class ReferencePath:
if __name__ == '__main__': if __name__ == '__main__':
# Select Path | 'Race' or 'Q' # Select Track | 'Real_Track' or 'Sim_Track'
path = 'Q' path = 'Sim_Track'
if path == 'Sim_Track':
# Load map file
map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
# Create Map
if path == 'Race':
map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
# Specify waypoints # Specify waypoints
wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
1.25, -0.75, -0.75, -0.25] 1.25, -0.75, -0.75, -0.25]
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
-1.5, -1.5] -1.5, -1.5]
# Specify path resolution # Specify path resolution
path_resolution = 0.05 # m / wp path_resolution = 0.05 # m / wp
# Create reference path
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=0.15, smoothing_distance=5, max_width=0.15,
circular=True) circular=True)
# Add obstacles # Add obstacles
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08) obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
@ -708,19 +684,26 @@ if __name__ == '__main__':
reference_path.map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, reference_path.map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
obs8]) obs8])
elif path == 'Q': elif path == 'Real_Track':
map = Map(file_path='map_floor2.png')
# wp_x = [-9.169, 11.9, 7.3, -6.95] # Load map file
# wp_y = [-15.678, 10.9, 14.5, -3.31] map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
resolution=0.06)
# Specify waypoints
wp_x = [-1.62, -6.04, -6.6, -5.36, -2.0, 5.9, wp_x = [-1.62, -6.04, -6.6, -5.36, -2.0, 5.9,
11.9, 7.3, 0.0, -1.62] 11.9, 7.3, 0.0, -1.62]
wp_y = [3.24, -1.4, -3.0, -5.36, -6.65, 3.5, wp_y = [3.24, -1.4, -3.0, -5.36, -6.65, 3.5,
10.9, 14.5, 5.2, 3.24] 10.9, 14.5, 5.2, 3.24]
# Specify path resolution # Specify path resolution
path_resolution = 0.2 # m / wp path_resolution = 0.2 # m / wp
# Create reference path
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=2.0, smoothing_distance=5, max_width=2.0,
circular=True) circular=True)
# Add obstacles and bounds to map # Add obstacles and bounds to map
cone1 = Obstacle(-5.9, -2.9, 0.2) cone1 = Obstacle(-5.9, -2.9, 0.2)
cone2 = Obstacle(-2.3, -5.9, 0.2) cone2 = Obstacle(-2.3, -5.9, 0.2)
@ -731,16 +714,11 @@ if __name__ == '__main__':
table3 = Obstacle(4.30, 3.22, 0.2) table3 = Obstacle(4.30, 3.22, 0.2)
obstacle_list = [cone1, cone2, cone3, cone4, table1, table2, table3] obstacle_list = [cone1, cone2, cone3, cone4, table1, table2, table3]
map.add_obstacles(obstacle_list) map.add_obstacles(obstacle_list)
# bound1 = ((2.25, -2.5), (1.55, 1.0))
# bound2 = ((1.56, 1.25), (3.64, 0.75))
# bound3 = ((4.46, 3.06), (7.51, 6.9))
# bound4 = ((4.18, 3.03), (1.95, 3.26))
# bound5 = ((-3.26, -0.21), (7.29, 13.16))
bound1 = ((-0.02, -2.72), (1.5, 1.0)) bound1 = ((-0.02, -2.72), (1.5, 1.0))
bound2 = ((4.43, 3.07), (1.5, 1.0)) bound2 = ((4.43, 3.07), (1.5, 1.0))
bound3 = ((4.43, 3.07), (7.5, 6.93)) bound3 = ((4.43, 3.07), (7.5, 6.93))
bound4 = ((7.28, 13.37), (-3.32, -0.12)) bound4 = ((7.28, 13.37), (-3.32, -0.12))
boundary_list = [bound1, bound2, bound3, bound4] boundary_list = [bound1, bound2, bound3, bound4]
map.add_boundary(boundary_list) map.add_boundary(boundary_list)

View File

@ -1,6 +1,6 @@
from map import Map from map import Map, Obstacle
import numpy as np import numpy as np
from reference_path import ReferencePath, Obstacle from reference_path import ReferencePath
from spatial_bicycle_models import BicycleModel from spatial_bicycle_models import BicycleModel
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from MPC import MPC from MPC import MPC
@ -9,98 +9,120 @@ from scipy import sparse
if __name__ == '__main__': if __name__ == '__main__':
# Select Simulation Mode | 'Race' or 'Q' # Select Simulation Mode | 'Sim_Track' or 'Real_Track'
sim_mode = 'Race' sim_mode = 'Sim_Track'
# Simulation Environment. Mini-Car on track specifically designed to show-
# case time-optimal driving.
if sim_mode == 'Sim_Track':
# Load map file
map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
# Create Map
if sim_mode == 'Race':
map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
# Specify waypoints # Specify waypoints
wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
1.25, -0.75, -0.75, -0.25] 1.25, -0.75, -0.75, -0.25]
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
-1.5, -1.5] -1.5, -1.5]
# Specify path resolution # Specify path resolution
path_resolution = 0.05 # m / wp path_resolution = 0.05 # m / wp
# Create smoothed reference path # Create smoothed reference path
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=0.23, smoothing_distance=5, max_width=0.23,
circular=True) circular=True)
# Add obstacles
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06)
map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, # Add obstacles
obs8, obs9]) use_obstacles = False
elif sim_mode == 'Q': if use_obstacles:
map = Map(file_path='map_floor2.png') obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06)
map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
obs8, obs9])
# Instantiate motion model
car = BicycleModel(length=0.12, width=0.06,
reference_path=reference_path, Ts=0.05)
# Real-World Environment. Track used for testing the algorithm on a 1:12
# RC car.
elif sim_mode == 'Real_Track':
# Load map file
map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
resolution=0.06)
# Specify waypoints
wp_x = [-9.169, 11.9, 7.3, -6.95] wp_x = [-9.169, 11.9, 7.3, -6.95]
wp_y = [-15.678, 10.9, 14.5, -3.31] wp_y = [-15.678, 10.9, 14.5, -3.31]
# Specify path resolution # Specify path resolution
path_resolution = 0.20 # m / wp path_resolution = 0.20 # m / wp
# Create smoothed reference path # Create smoothed reference path
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=1.50, smoothing_distance=5, max_width=1.50,
circular=False) circular=False)
obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) # Add obstacles
obs4 = Obstacle(cx=2.0, cy=-0.2, radius=0.25) add_obstacles = False
obs8 = Obstacle(cx=6.0, cy=5.0, radius=0.3) if add_obstacles:
obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
map.add_obstacles([obs1, obs2, obs4, obs8, obs9]) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25)
obs4 = Obstacle(cx=2.0, cy=-0.2, radius=0.25)
obs8 = Obstacle(cx=6.0, cy=5.0, radius=0.3)
obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3)
map.add_obstacles([obs1, obs2, obs4, obs8, obs9])
# Instantiate motion model
car = BicycleModel(length=0.30, width=0.20,
reference_path=reference_path, Ts=0.05)
else: else:
print('Invalid Simulation Mode!') print('Invalid Simulation Mode!')
map, wp_x, wp_y, path_resolution, reference_path \ map, wp_x, wp_y, path_resolution, reference_path, car \
= None, None, None, None, None = None, None, None, None, None, None
exit(1) exit(1)
################
# Motion Model #
################
# Initial state
e_y_0 = 0.0
e_psi_0 = 0.0
t_0 = 0.0
V_MAX = 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)
############## ##############
# Controller # # Controller #
############## ##############
N = 30 N = 30
Q = sparse.diags([0.3, 0.0, 0.0]) Q = sparse.diags([1.0, 0.0, 0.0])
R = sparse.diags([0.5, 0.0]) R = sparse.diags([0.5, 0.0])
QN = sparse.diags([0.3, 0.0, 0.0]) QN = sparse.diags([1.0, 0.0, 0.0])
InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]),
'umax': np.array([V_MAX, np.tan(0.66)/car.l])} v_max = 1.0 # m/s
delta_max = 0.66 # rad
ay_max = 4.0 # m/s^2
InputConstraints = {'umin': np.array([0.0, -np.tan(delta_max)/car.length]),
'umax': np.array([v_max, np.tan(delta_max)/car.length])}
StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),
'xmax': 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) mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, ay_max)
# Compute speed profile # Compute speed profile
SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5, a_min = -0.1 # m/s^2
'v_min': 0, 'v_max': V_MAX, 'ay_max': 4.0} a_max = 0.5 # m/s^2
SpeedProfileConstraints = {'a_min': a_min, 'a_max': a_max,
'v_min': 0.0, 'v_max': v_max, 'ay_max': ay_max}
car.reference_path.compute_speed_profile(SpeedProfileConstraints) car.reference_path.compute_speed_profile(SpeedProfileConstraints)
############## ##############
# Simulation # # Simulation #
############## ##############
# Sampling time # Set simulation time to zero
Ts = 0.05 t = 0.0
t = 0
car.set_sampling_time(Ts)
# Logging containers # Logging containers
x_log = [car.temporal_state.x] x_log = [car.temporal_state.x]
@ -122,7 +144,7 @@ if __name__ == '__main__':
v_log.append(u[0]) v_log.append(u[0])
# Increase simulation time # Increase simulation time
t += Ts t += car.Ts
# Plot path and drivable area # Plot path and drivable area
reference_path.show() reference_path.show()
@ -131,8 +153,7 @@ if __name__ == '__main__':
car.show() car.show()
# Plot MPC prediction # Plot MPC prediction
if mpc.current_prediction is not None: mpc.show_prediction()
mpc.show_prediction()
# Set figure title # Set figure title
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '