mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
first release MPC sample
This commit is contained in:
Atsushi Sakai
3
.gitmodules
vendored
3
.gitmodules
vendored
@@ -31,3 +31,6 @@
|
||||
[submodule "PathTracking/lqr_speed_steer_control/pycubicspline"]
|
||||
path = PathTracking/lqr_speed_steer_control/pycubicspline
|
||||
url = https://github.com/AtsushiSakai/pycubicspline
|
||||
[submodule "PathTracking/model_predictive_speed_and_steer_control/pycubicspline"]
|
||||
path = PathTracking/model_predictive_speed_and_steer_control/pycubicspline
|
||||
url = https://github.com/AtsushiSakai/pycubicspline
|
||||
|
||||
@@ -1,15 +1,553 @@
|
||||
"""
|
||||
|
||||
Path tracking simulation with model predictive control for speed and steer control
|
||||
Path tracking simulation with iterative linear model predictive control for speed and steer control
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
import cvxpy
|
||||
import matplotlib.pyplot as plt
|
||||
from pycubicspline import pycubicspline
|
||||
# from matplotrecorder import matplotrecorder
|
||||
|
||||
nx = 4 # x = x, y, v, yaw
|
||||
nu = 2 # a = [accel, steer]
|
||||
T = 5 # horizon length
|
||||
|
||||
# mpc parameters
|
||||
R = np.diag([1.0, 0.01])
|
||||
Rd = np.diag([0.01, 0.01])
|
||||
Q = np.diag([1.0, 1.0, 0.5, 10.0])
|
||||
Qf = Q
|
||||
goal_dis = 1.5 # goal distance
|
||||
stop_speed = 0.5 / 3.6 # stop speed
|
||||
maxtime = 500.0 # max simulation time
|
||||
max_accel = 1.0 # maximum accel
|
||||
|
||||
# iterative paramter
|
||||
maxiter = 3
|
||||
du_th = 0.1
|
||||
|
||||
target_speed = 20.0 / 3.6
|
||||
|
||||
show_animation = True
|
||||
# show_animation = False
|
||||
|
||||
dt = 0.2 # [s] time tick
|
||||
WB = 6.0 # [m] Wheel base
|
||||
|
||||
maxsteer = math.radians(45.0) # maximum steering angle
|
||||
maxdsteer = math.radians(7.0) # maximum steering speed
|
||||
maxspeed = 55.0 / 3.6 # maximum speed
|
||||
minspeed = -20.0 / 3.6 # minmum speed
|
||||
|
||||
predelta = None
|
||||
|
||||
|
||||
class State:
|
||||
"""
|
||||
vehicle state class
|
||||
"""
|
||||
|
||||
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.yaw = yaw
|
||||
self.v = v
|
||||
|
||||
|
||||
def pi_2_pi(angle):
|
||||
while(angle > math.pi):
|
||||
angle = angle - 2.0 * math.pi
|
||||
|
||||
while(angle < -math.pi):
|
||||
angle = angle + 2.0 * math.pi
|
||||
|
||||
return angle
|
||||
|
||||
|
||||
def get_linear_model_matrix(v, phi, delta):
|
||||
|
||||
A = np.matrix(np.zeros((nx, nx)))
|
||||
A[0, 0] = 1.0
|
||||
A[1, 1] = 1.0
|
||||
A[2, 2] = 1.0
|
||||
A[3, 3] = 1.0
|
||||
A[0, 2] = dt * math.cos(phi)
|
||||
A[0, 3] = - dt * v * math.sin(phi)
|
||||
A[1, 2] = dt * math.sin(phi)
|
||||
A[1, 3] = dt * v * math.cos(phi)
|
||||
A[3, 2] = dt * math.tan(delta)
|
||||
|
||||
B = np.matrix(np.zeros((nx, nu)))
|
||||
B[2, 0] = dt
|
||||
B[3, 1] = dt * v / (WB * math.cos(delta) ** 2)
|
||||
|
||||
C = np.matrix(np.zeros((nx, 1)))
|
||||
C[0, 0] = dt * v * math.sin(phi) * phi
|
||||
C[1, 0] = - dt * v * math.cos(phi) * phi
|
||||
C[3, 0] = v * delta / (WB * math.cos(delta) ** 2)
|
||||
|
||||
return A, B, C
|
||||
|
||||
|
||||
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
|
||||
|
||||
LENGTH = 12.0
|
||||
WIDTH = 6.0
|
||||
BACKTOWHEEL = 2.5
|
||||
WHEEL_LEN = 1.0
|
||||
WHEEL_WIDTH = 0.5
|
||||
TREAD = 2.0
|
||||
|
||||
outline = np.matrix([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
|
||||
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
|
||||
|
||||
fr_wheel = np.matrix([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
|
||||
[-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
|
||||
|
||||
rr_wheel = np.copy(fr_wheel)
|
||||
|
||||
fl_wheel = np.copy(fr_wheel)
|
||||
fl_wheel[1, :] *= -1
|
||||
rl_wheel = np.copy(rr_wheel)
|
||||
rl_wheel[1, :] *= -1
|
||||
|
||||
Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
|
||||
[-math.sin(yaw), math.cos(yaw)]])
|
||||
Rot2 = np.matrix([[math.cos(steer), math.sin(steer)],
|
||||
[-math.sin(steer), math.cos(steer)]])
|
||||
|
||||
fr_wheel = (fr_wheel.T * Rot2).T
|
||||
fl_wheel = (fl_wheel.T * Rot2).T
|
||||
fr_wheel[0, :] += WB
|
||||
fl_wheel[0, :] += WB
|
||||
|
||||
fr_wheel = (fr_wheel.T * Rot1).T
|
||||
fl_wheel = (fl_wheel.T * Rot1).T
|
||||
|
||||
outline = (outline.T * Rot1).T
|
||||
rr_wheel = (rr_wheel.T * Rot1).T
|
||||
rl_wheel = (rl_wheel.T * Rot1).T
|
||||
|
||||
outline[0, :] += x
|
||||
outline[1, :] += y
|
||||
fr_wheel[0, :] += x
|
||||
fr_wheel[1, :] += y
|
||||
rr_wheel[0, :] += x
|
||||
rr_wheel[1, :] += y
|
||||
fl_wheel[0, :] += x
|
||||
fl_wheel[1, :] += y
|
||||
rl_wheel[0, :] += x
|
||||
rl_wheel[1, :] += y
|
||||
|
||||
plt.plot(np.array(outline[0, :]).flatten(),
|
||||
np.array(outline[1, :]).flatten(), truckcolor)
|
||||
plt.plot(np.array(fr_wheel[0, :]).flatten(),
|
||||
np.array(fr_wheel[1, :]).flatten(), truckcolor)
|
||||
plt.plot(np.array(rr_wheel[0, :]).flatten(),
|
||||
np.array(rr_wheel[1, :]).flatten(), truckcolor)
|
||||
plt.plot(np.array(fl_wheel[0, :]).flatten(),
|
||||
np.array(fl_wheel[1, :]).flatten(), truckcolor)
|
||||
plt.plot(np.array(rl_wheel[0, :]).flatten(),
|
||||
np.array(rl_wheel[1, :]).flatten(), truckcolor)
|
||||
plt.plot(x, y, "*")
|
||||
|
||||
|
||||
def update_state(state, a, delta):
|
||||
|
||||
# input check
|
||||
if delta >= maxsteer:
|
||||
delta = maxsteer
|
||||
elif delta <= -maxsteer:
|
||||
delta = -maxsteer
|
||||
|
||||
global predelta
|
||||
if predelta is not None:
|
||||
if (delta - predelta) >= (maxdsteer * dt):
|
||||
delta = predelta + maxdsteer * dt
|
||||
elif (delta - predelta) <= -(maxdsteer * dt):
|
||||
delta = predelta - maxdsteer * dt
|
||||
predelta = delta
|
||||
|
||||
state.x = state.x + state.v * math.cos(state.yaw) * dt
|
||||
state.y = state.y + state.v * math.sin(state.yaw) * dt
|
||||
state.yaw = state.yaw + state.v / WB * math.tan(delta) * dt
|
||||
state.v = state.v + a * dt
|
||||
|
||||
if state. v > maxspeed:
|
||||
state.v = maxspeed
|
||||
elif state. v < minspeed:
|
||||
state.v = minspeed
|
||||
|
||||
return state
|
||||
|
||||
|
||||
def get_nparray_from_matrix(x):
|
||||
"""
|
||||
get build-in list from matrix
|
||||
"""
|
||||
return np.array(x).flatten()
|
||||
|
||||
|
||||
def calc_nearest_index(state, cx, cy, cyaw):
|
||||
dx = [state.x - icx for icx in cx]
|
||||
dy = [state.y - icy for icy in cy]
|
||||
|
||||
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
|
||||
|
||||
mind = min(d)
|
||||
|
||||
ind = d.index(mind)
|
||||
|
||||
dxl = cx[ind] - state.x
|
||||
dyl = cy[ind] - state.y
|
||||
|
||||
angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
|
||||
if angle < 0:
|
||||
mind *= -1
|
||||
|
||||
return ind, mind
|
||||
|
||||
|
||||
def predict_motion(x0, oa, od, xref):
|
||||
xbar = xref * 0.0
|
||||
for i in range(len(x0)):
|
||||
xbar[i, 0] = x0[i]
|
||||
|
||||
state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2])
|
||||
for (ai, di, i) in zip(oa, od, range(1, T + 1)):
|
||||
state = update_state(state, ai, di)
|
||||
xbar[0, i] = state.x
|
||||
xbar[1, i] = state.y
|
||||
xbar[2, i] = state.v
|
||||
xbar[3, i] = state.yaw
|
||||
|
||||
return xbar
|
||||
|
||||
|
||||
def iterative_linear_mpc_control(xref, x0, dref, oa, od):
|
||||
"""
|
||||
MPC contorl with updating operational point iteraitvely
|
||||
"""
|
||||
|
||||
if oa is None or od is None:
|
||||
oa = [0.0] * T
|
||||
od = [0.0] * T
|
||||
|
||||
for i in range(maxiter):
|
||||
xbar = predict_motion(x0, oa, od, xref)
|
||||
poa, pod = oa[:], od[:]
|
||||
oa, od, ox, oy, oyaw, ov = linear_mpc_control(xref, xbar, x0, dref)
|
||||
du = sum(abs(oa - poa)) + sum(abs(od - pod)) # calc u change value
|
||||
if du <= du_th:
|
||||
break
|
||||
else:
|
||||
print("Iterative is max iter")
|
||||
|
||||
return oa, od, ox, oy, oyaw, ov
|
||||
|
||||
|
||||
def linear_mpc_control(xref, xbar, x0, dref):
|
||||
"""
|
||||
linear mpc control
|
||||
|
||||
xref: reference point
|
||||
xbar: operational point
|
||||
x0: initial state
|
||||
dref: reference steer angle
|
||||
"""
|
||||
|
||||
x = cvxpy.Variable(nx, T + 1)
|
||||
u = cvxpy.Variable(nu, T)
|
||||
|
||||
cost = 0.0
|
||||
constraints = []
|
||||
|
||||
for t in range(T):
|
||||
cost += cvxpy.quad_form(u[:, t], R)
|
||||
|
||||
if t != 0:
|
||||
cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q)
|
||||
|
||||
A, B, C = get_linear_model_matrix(
|
||||
xbar[2, t], xbar[3, t], dref[0, t])
|
||||
constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C]
|
||||
|
||||
if t < (T - 1):
|
||||
cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
|
||||
constraints += [cvxpy.abs(u[1, t + 1] - u[1, t])
|
||||
< maxdsteer * dt]
|
||||
|
||||
cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
|
||||
|
||||
constraints += [x[:, 0] == x0]
|
||||
constraints += [x[2, :] <= maxspeed]
|
||||
constraints += [x[2, :] >= minspeed]
|
||||
constraints += [cvxpy.abs(u[0, :]) < max_accel]
|
||||
constraints += [cvxpy.abs(u[1, :]) < maxsteer]
|
||||
|
||||
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
|
||||
prob.solve(verbose=False)
|
||||
|
||||
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
|
||||
ox = get_nparray_from_matrix(x.value[0, :])
|
||||
oy = get_nparray_from_matrix(x.value[1, :])
|
||||
ov = get_nparray_from_matrix(x.value[2, :])
|
||||
oyaw = get_nparray_from_matrix(x.value[3, :])
|
||||
oa = get_nparray_from_matrix(u.value[0, :])
|
||||
odelta = get_nparray_from_matrix(u.value[1, :])
|
||||
|
||||
else:
|
||||
print("Error: Cannot solve mpc..")
|
||||
oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None
|
||||
|
||||
return oa, odelta, ox, oy, oyaw, ov
|
||||
|
||||
|
||||
def calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, pind):
|
||||
xref = np.zeros((nx, T + 1))
|
||||
dref = np.zeros((1, T + 1))
|
||||
ncourse = len(cx)
|
||||
|
||||
ind, _ = calc_nearest_index(state, cx, cy, cyaw)
|
||||
|
||||
if pind >= ind:
|
||||
ind = pind
|
||||
|
||||
xref[0, 0] = cx[ind]
|
||||
xref[1, 0] = cy[ind]
|
||||
xref[2, 0] = sp[ind]
|
||||
xref[3, 0] = cyaw[ind]
|
||||
dref[0, 0] = 0.0 # steer operational point should be 0
|
||||
|
||||
travel = 0.0
|
||||
|
||||
for i in range(T + 1):
|
||||
travel += abs(state.v) * dt
|
||||
dind = int(round(travel / dl))
|
||||
|
||||
if (ind + dind) < ncourse:
|
||||
xref[0, i] = cx[ind + dind]
|
||||
xref[1, i] = cy[ind + dind]
|
||||
xref[2, i] = sp[ind + dind]
|
||||
xref[3, i] = cyaw[ind + dind]
|
||||
dref[0, i] = 0.0
|
||||
else:
|
||||
xref[0, i] = cx[ncourse - 1]
|
||||
xref[1, i] = cy[ncourse - 1]
|
||||
xref[2, i] = sp[ncourse - 1]
|
||||
xref[3, i] = cyaw[ncourse - 1]
|
||||
dref[0, i] = 0.0
|
||||
|
||||
return xref, ind, dref
|
||||
|
||||
|
||||
def check_goal(state, goal, tind, nind):
|
||||
|
||||
# check goal
|
||||
dx = state.x - goal[0]
|
||||
dy = state.y - goal[1]
|
||||
d = math.sqrt(dx ** 2 + dy ** 2)
|
||||
# print(d)
|
||||
|
||||
if (d <= goal_dis):
|
||||
isgoal = True
|
||||
else:
|
||||
isgoal = False
|
||||
|
||||
if abs(tind - nind) >= 5:
|
||||
isgoal = False
|
||||
|
||||
if (abs(state.v) <= stop_speed):
|
||||
isstop = True
|
||||
else:
|
||||
isstop = False
|
||||
|
||||
if isgoal and isstop:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def do_simulation(cx, cy, cyaw, ck, sp, dl):
|
||||
"""
|
||||
Simulation
|
||||
|
||||
cx: course x position list
|
||||
cy: course y position list
|
||||
cy: course yaw position list
|
||||
ck: course curvature list
|
||||
sp: speed profile
|
||||
dl: course tick [m]
|
||||
|
||||
"""
|
||||
|
||||
goal = [cx[-1], cy[-1]]
|
||||
state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
|
||||
|
||||
time = 0.0
|
||||
x = [state.x]
|
||||
y = [state.y]
|
||||
yaw = [state.yaw]
|
||||
v = [state.v]
|
||||
t = [0.0]
|
||||
d = [0.0]
|
||||
a = [0.0]
|
||||
target_ind, _ = calc_nearest_index(state, cx, cy, cyaw)
|
||||
|
||||
odelta, oa = None, None
|
||||
|
||||
cyaw = smooth_yaw(cyaw)
|
||||
|
||||
while maxtime >= time:
|
||||
xref, target_ind, dref = calc_ref_trajectory(
|
||||
state, cx, cy, cyaw, ck, sp, dl, target_ind)
|
||||
|
||||
x0 = [state.x, state.y, state.v, state.yaw] # current state
|
||||
|
||||
oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control(
|
||||
xref, x0, dref, oa, odelta)
|
||||
|
||||
if odelta is not None:
|
||||
di, ai = odelta[0], oa[0]
|
||||
|
||||
state = update_state(state, ai, di)
|
||||
time = time + dt
|
||||
|
||||
x.append(state.x)
|
||||
y.append(state.y)
|
||||
yaw.append(state.yaw)
|
||||
v.append(state.v)
|
||||
t.append(time)
|
||||
d.append(di)
|
||||
a.append(ai)
|
||||
|
||||
if check_goal(state, goal, target_ind, len(cx)):
|
||||
print("Goal")
|
||||
break
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
if ox is not None:
|
||||
plt.plot(ox, oy, "xr", label="MPC")
|
||||
plt.plot(cx, cy, "-r", label="course")
|
||||
plt.plot(x, y, "ob", label="trajectory")
|
||||
plt.plot(xref[0, :], xref[1, :], "xk", label="xref")
|
||||
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
|
||||
plot_car(state.x, state.y, state.yaw, steer=di)
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
|
||||
# wsize = 50.0
|
||||
# set_xlim([-wsize + state.x, wsize + state.x])
|
||||
# set_ylim([-wsize + state.y, wsize + state.y])
|
||||
plt.title("Time[s]:" + str(round(time, 2)) +
|
||||
", speed[km/h]:" + str(round(state.v * 3.6, 2)))
|
||||
plt.pause(0.0001)
|
||||
|
||||
return t, x, y, yaw, v, d, a
|
||||
|
||||
|
||||
def calc_speed_profile(cx, cy, cyaw, target_speed):
|
||||
|
||||
speed_profile = [target_speed] * len(cx)
|
||||
direction = 1.0 # forward
|
||||
|
||||
# Set stop point
|
||||
for i in range(len(cx) - 1):
|
||||
dx = cx[i + 1] - cx[i]
|
||||
dy = cy[i + 1] - cy[i]
|
||||
|
||||
move_direction = math.atan2(dy, dx)
|
||||
|
||||
if dx != 0.0 and dy != 0.0:
|
||||
dangle = abs(pi_2_pi(move_direction - cyaw[i]))
|
||||
if dangle >= math.pi / 4.0:
|
||||
direction = -1.0
|
||||
else:
|
||||
direction = 1.0
|
||||
|
||||
if direction != 1.0:
|
||||
speed_profile[i] = - target_speed
|
||||
else:
|
||||
speed_profile[i] = target_speed
|
||||
|
||||
speed_profile[-1] = 0.0
|
||||
|
||||
return speed_profile
|
||||
|
||||
|
||||
def smooth_yaw(yaw):
|
||||
|
||||
for i in range(len(yaw) - 1):
|
||||
dyaw = yaw[i + 1] - yaw[i]
|
||||
while dyaw >= math.pi / 2.0:
|
||||
yaw[i + 1] -= math.pi * 2.0
|
||||
dyaw = yaw[i + 1] - yaw[i]
|
||||
while dyaw <= -math.pi / 2.0:
|
||||
yaw[i + 1] += math.pi * 2.0
|
||||
dyaw = yaw[i + 1] - yaw[i]
|
||||
|
||||
return yaw
|
||||
|
||||
|
||||
def get_forward_course(dl):
|
||||
ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
|
||||
ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
|
||||
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=dl)
|
||||
|
||||
return cx, cy, cyaw, ck
|
||||
|
||||
|
||||
def get_switch_back_course(dl):
|
||||
ax = [0.0, 60.0, 125.0, 50.0, 75.0]
|
||||
ay = [0.0, 0.0, 50.0, 65.0, 30.0]
|
||||
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=dl)
|
||||
ax = [75.0, 30.0, 0.0, 0.0]
|
||||
ay = [30.0, 50.0, 10.0, 1.0]
|
||||
cx2, cy2, cyaw2, ck2, s2 = pycubicspline.calc_spline_course(ax, ay, ds=dl)
|
||||
cyaw2 = [i - math.pi for i in cyaw2]
|
||||
cx.extend(cx2)
|
||||
cy.extend(cy2)
|
||||
cyaw.extend(cyaw2)
|
||||
ck.extend(ck2)
|
||||
|
||||
return cx, cy, cyaw, ck
|
||||
|
||||
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
dl = 1.0 # course tick
|
||||
# cx, cy, cyaw, ck = get_forward_course(dl)
|
||||
cx, cy, cyaw, ck = get_switch_back_course(dl)
|
||||
|
||||
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
|
||||
|
||||
t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl)
|
||||
|
||||
if show_animation:
|
||||
plt.subplots()
|
||||
plt.plot(cx, cy, "-r", label="spline")
|
||||
plt.plot(x, y, "-g", label="tracking")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plt.xlabel("x[m]")
|
||||
plt.ylabel("y[m]")
|
||||
plt.legend()
|
||||
|
||||
plt.subplots()
|
||||
plt.plot(t, v, "-r", label="speed")
|
||||
plt.grid(True)
|
||||
plt.xlabel("Time [s]")
|
||||
plt.ylabel("Speed [kmh]")
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user