12345qiupeng
/
Multi-Purpose-MPC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add attribute dynamic_border_cells to reference path.

master
matssteinweg 2019-12-08 14:57:35 +01:00
parent dc7628ee8d
commit d25de2e0c2
4 changed files with 88 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
MPC.py
View File

@ -112,10 +112,13 @@ class MPC:
umax_dyn[self.nu*n] = vmax_dyn
# Compute dynamic constraints on e_y
ub, lb, cells = self.model.reference_path.update_path_constraints(
self.model.wp_id, self.N+1, 2*self.model.safety_margin[1], 0.05)
xmin_dyn[::self.nx] = lb
xmax_dyn[::self.nx] = ub
ub, lb = self.model.reference_path.update_path_constraints(
self.model.wp_id+1, self.N, 2*self.model.safety_margin[1],
self.model.safety_margin[1])
xmin_dyn[0] = self.model.spatial_state.e_y
xmax_dyn[0] = self.model.spatial_state.e_y
xmin_dyn[self.nx::self.nx] = lb
xmax_dyn[self.nx::self.nx] = ub
# Get equality matrix
Ax = sparse.kron(sparse.eye(self.N + 1),

98
reference_path.py
View File

@ -11,7 +11,7 @@ import osqp
DRIVABLE_AREA = '#BDC3C7'
WAYPOINTS = '#D0D3D4'
OBSTACLE = '#2E4053'
PATH_CONSTRAINTS = '#F5B041'
############
# Waypoint #
@ -40,7 +40,8 @@ class Waypoint:
# waypoint orientation
self.lb = None
self.ub = None
self.border_cells = None
self.static_border_cells = None
self.dynamic_border_cells = None
def __sub__(self, other):
"""
@ -264,7 +265,8 @@ class ReferencePath:
wp.lb = -1 * width_info[2] # minus can be assumed as waypoints
# represent center-line of the path
# Set border cells of waypoint
wp.border_cells = (width_info[1], width_info[3])
wp.static_border_cells = (width_info[1], width_info[3])
wp.dynamic_border_cells = (width_info[1], width_info[3])
def _get_min_width(self, wp, t_x, t_y, max_width):
"""
@ -316,6 +318,8 @@ class ReferencePath:
"""
Compute a speed profile for the path. Assign a reference velocity
to each waypoint based on its curvature.
:param Constraints: constraints on acceleration and velocity
curvature of the path
"""
# Set optimization horizon
@ -336,8 +340,6 @@ class ReferencePath:
# Iterate over horizon
for i in range(N):
look_ahead = 30
# Get information about current waypoint
current_waypoint = self.get_waypoint(i)
next_waypoint = self.get_waypoint(i+1)
@ -345,9 +347,6 @@ class ReferencePath:
li = next_waypoint - current_waypoint
# curvature of waypoint
ki = current_waypoint.kappa
if np.abs(ki) <= 0.1:
kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]]
ki = np.mean(kis)
# Fill operator matrix
# dynamics of acceleration
@ -436,8 +435,8 @@ class ReferencePath:
plt.yticks([])
# Plot map in gray-scale and set extent to match world coordinates
#canvas = np.ones(self.map.data.shape)
canvas = np.flipud(self.map.data)
canvas = np.ones(self.map.data.shape)
#canvas = np.flipud(self.map.data)
plt.imshow(canvas, cmap='gray',
extent=[self.map.origin[0], self.map.origin[0] +
self.map.width * self.map.resolution,
@ -450,10 +449,10 @@ class ReferencePath:
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])
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])
wp_ub_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints])
wp_ub_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints])
wp_lb_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints])
wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints])
# Plot waypoints
plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10)
@ -468,18 +467,18 @@ class ReferencePath:
headwidth=1, headlength=0)
# Plot border of path
bl_x = np.array([wp.border_cells[0][0] for wp in
bl_x = np.array([wp.static_border_cells[0][0] for wp in
self.waypoints] +
[self.waypoints[0].border_cells[0][0]])
bl_y = np.array([wp.border_cells[0][1] for wp in
[self.waypoints[0].static_border_cells[0][0]])
bl_y = np.array([wp.static_border_cells[0][1] for wp in
self.waypoints] +
[self.waypoints[0].border_cells[0][1]])
br_x = np.array([wp.border_cells[1][0] for wp in
[self.waypoints[0].static_border_cells[0][1]])
br_x = np.array([wp.static_border_cells[1][0] for wp in
self.waypoints] +
[self.waypoints[0].border_cells[1][0]])
br_y = np.array([wp.border_cells[1][1] for wp in
[self.waypoints[0].static_border_cells[1][0]])
br_y = np.array([wp.static_border_cells[1][1] for wp in
self.waypoints] +
[self.waypoints[0].border_cells[1][1]])
[self.waypoints[0].static_border_cells[1][1]])
# If circular path, connect start and end point
if self.circular:
@ -492,6 +491,19 @@ class ReferencePath:
plt.plot((bl_x[-2], br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE)
plt.plot((bl_x[0], br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE)
# Plot dynamic path constraints
# Get x and y locations of border cells for upper and lower bound
wp_ub_x = np.array(
[wp.dynamic_border_cells[0][0] for wp in self.waypoints])
wp_ub_y = np.array(
[wp.dynamic_border_cells[0][1] for wp in self.waypoints])
wp_lb_x = np.array(
[wp.dynamic_border_cells[1][0] for wp in self.waypoints])
wp_lb_y = np.array(
[wp.dynamic_border_cells[1][1] for wp in self.waypoints])
plt.plot(wp_ub_x, wp_ub_y, color=PATH_CONSTRAINTS)
plt.plot(wp_lb_x, wp_lb_y, color=PATH_CONSTRAINTS)
# Plot obstacles
for obstacle in self.obstacles:
obstacle.show()
@ -508,10 +520,10 @@ class ReferencePath:
free_segments = []
# Get waypoint's border cells in map coordinates
ub_p = self.map.w2m(wp.border_cells[0][0],
wp.border_cells[0][1])
lb_p = self.map.w2m(wp.border_cells[1][0],
wp.border_cells[1][1])
ub_p = self.map.w2m(wp.static_border_cells[0][0],
wp.static_border_cells[0][1])
lb_p = self.map.w2m(wp.static_border_cells[1][0],
wp.static_border_cells[1][1])
# Compute path from left border cell to right border cell
x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1])
@ -562,12 +574,13 @@ class ReferencePath:
ub_hor = []
lb_hor = []
border_cells_hor = []
border_cells_hor_sm = []
# Iterate over horizon
for n in range(N):
# get corresponding waypoint
wp = self.waypoints[wp_id+n]
wp = self.get_waypoint(wp_id+n)
# Get list of free segments
free_segments = self._compute_free_segments(wp, min_width)
@ -586,7 +599,7 @@ class ReferencePath:
ub_pw, lb_pw = list(ub_pw), list(lb_pw)
# Project border cells onto new waypoint in path direction
wp_prev = self.waypoints[wp_id+n-1]
wp_prev = self.get_waypoint(wp_id+n-1)
delta_s = wp_prev - wp
ub_pw[0] += delta_s * np.cos(wp_prev.psi)
ub_pw[1] += delta_s * np.cos(wp_prev.psi)
@ -660,14 +673,24 @@ class ReferencePath:
angle_ub)
lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(
angle_lb)
bound_cells_sm = (ub_ls, lb_ls)
# Compute cell on bound for computed distance ub and lb
ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin(
angle_ub)
lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin(
angle_lb)
bound_cells = (ub_ls, lb_ls)
# Append results
ub_hor.append(ub)
lb_hor.append(lb)
border_cells_hor.append(list(bound_cells))
border_cells_hor_sm.append(list(bound_cells_sm))
return np.array(ub_hor), np.array(lb_hor), border_cells_hor
# Assign dynamic border cells to waypoints
wp.dynamic_border_cells = bound_cells_sm
return np.array(ub_hor), np.array(lb_hor)
if __name__ == '__main__':
@ -711,29 +734,22 @@ if __name__ == '__main__':
circular=False)
obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25)
obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15)
obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25)
obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1)
obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1)
obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1)
obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3)
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)
obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1)
reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10])
reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9])
else:
reference_path = None
print('Invalid path!')
exit(1)
ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.02, 0.01)
ub, lb, border_cells = reference_path.update_path_constraints(0,
reference_path.n_waypoints, 0.02, 0.01)
# Get x and y locations of border cells for upper and lower bound
for wp_id in range(reference_path.n_waypoints):
if ub[wp_id] > 0.0 and lb[wp_id] > 0.0:
print(wp_id)
reference_path.waypoints[wp_id].border_cells = border_cells[wp_id]
reference_path.show()
plt.show()

43
simulation.py
View File

@ -26,6 +26,17 @@ if __name__ == '__main__':
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=0.23,
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.05)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05)
obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05)
obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05)
reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
obs8])
elif sim_mode == 'Q':
map = Map(file_path='map_floor2.png')
wp_x = [-9.169, 11.9, 7.3, -6.95]
@ -36,25 +47,18 @@ if __name__ == '__main__':
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=1.50,
circular=False)
obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
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)
reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9])
else:
print('Invalid Simulation Mode!')
map, wp_x, wp_y, path_resolution, reference_path \
= None, None, None, None, None
exit(1)
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.07)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.07)
obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.07)
obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.08)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
obs9 = Obstacle(cx=0.7, cy=-0.1, radius=0.05)
obs10 = Obstacle(cx=1.1, cy=-0.6, radius=0.07)
reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
obs8, obs9, obs10])
################
# Motion Model #
################
@ -65,7 +69,7 @@ if __name__ == '__main__':
t_0 = 0.0
V_MAX = 2.5
car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path,
car = BicycleModel(length=0.56, width=0.33, reference_path=reference_path,
e_y=e_y_0, e_psi=e_psi_0, t=t_0)
##############
@ -83,7 +87,7 @@ if __name__ == '__main__':
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints)
# Compute speed profile
SpeedProfileConstraints = {'a_min': -1.0, 'a_max': 1.0,
SpeedProfileConstraints = {'a_min': -0.05, 'a_max': 0.5,
'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0}
car.reference_path.compute_speed_profile(SpeedProfileConstraints)
@ -92,7 +96,7 @@ if __name__ == '__main__':
##############
# Sampling time
Ts = 0.20
Ts = 0.05
t = 0
car.set_sampling_time(Ts)
@ -121,8 +125,8 @@ if __name__ == '__main__':
# Plot path and drivable area
reference_path.show()
plt.scatter(x_log, y_log, c=v_log, s=10)
plt.colorbar()
#plt.scatter(x_log, y_log, c=v_log, s=10)
#plt.colorbar()
# Plot MPC prediction
mpc.show_prediction()
@ -137,6 +141,5 @@ if __name__ == '__main__':
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
'{:.2f} s'.format(u[0], u[1], t))
plt.pause(0.00001)
print(min(v_log))
plt.pause(0.000001)
plt.show()

2
spatial_bicycle_models.py
View File

@ -240,7 +240,7 @@ class SpatialBicycleModel(ABC):
# Model ellipsoid around the car
length = self.l / np.sqrt(2)
width = self.w / np.sqrt(2)
width = self.w / np.sqrt(2) + 0.02
return length, width