add readme
This commit is contained in:
Shunichi09
parent
dcb9aa0689
commit
fe39636d39
|
|
@ -0,0 +1,37 @@
|
||||||
|
# Model Predictive Control for Vehicle model
|
||||||
|
This program is for controlling the vehicle model.
|
||||||
|
I implemented the steering control for vehicle by using Model Predictive Control.
|
||||||
|
|
||||||
|
# Model
|
||||||
|
Usually, the vehicle model is expressed by extremely complicated nonlinear equation.
|
||||||
|
Acoording to reference 1, I used the simple model as shown in following equation.
|
||||||
|
|
||||||
|
<a href="https://www.codecogs.com/eqnedit.php?latex=\begin{align*}&space;x[k+1]&space;=&space;x[k]&space;+&space;v\cos(\theta[k])dt&space;\\&space;y[k+1]&space;=&space;y[k]&space;+&space;v\sin(\theta[k])dt&space;\\&space;\theta[k+1]&space;=&space;\theta[k]&space;+&space;\frac{v&space;\tan{\delta[k]}}{L}dt\\&space;\delta[k+1]&space;=&space;\delta[k]&space;-&space;\tau^{-1}(\delta[k]-\delta_{input})dt&space;\end{align*}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\begin{align*}&space;x[k+1]&space;=&space;x[k]&space;+&space;v\cos(\theta[k])dt&space;\\&space;y[k+1]&space;=&space;y[k]&space;+&space;v\sin(\theta[k])dt&space;\\&space;\theta[k+1]&space;=&space;\theta[k]&space;+&space;\frac{v&space;\tan{\delta[k]}}{L}dt\\&space;\delta[k+1]&space;=&space;\delta[k]&space;-&space;\tau^{-1}(\delta[k]-\delta_{input})dt&space;\end{align*}" title="\begin{align*} x[k+1] = x[k] + v\cos(\theta[k])dt \\ y[k+1] = y[k] + v\sin(\theta[k])dt \\ \theta[k+1] = \theta[k] + \frac{v \tan{\delta[k]}}{L}dt\\ \delta[k+1] = \delta[k] - \tau^{-1}(\delta[k]-\delta_{input})dt \end{align*}" /></a>
|
||||||
|
|
||||||
|
However, it is still a nonlinear equation.
|
||||||
|
Therefore, I assume that the car is tracking the reference trajectory.
|
||||||
|
If we get the assumption, the model can turn to linear model by using the path's curvatures.
|
||||||
|
|
||||||
|
<a href="https://www.codecogs.com/eqnedit.php?latex=\boldsymbol{X}[k+1]&space;=&space;\begin{bmatrix}&space;1&space;&&space;vdt&space;&&space;0&space;\\&space;0&space;&&space;1&space;&&space;\frac{vdt}{L&space;\&space;cos^{2}&space;\delta_r}&space;\\&space;0&space;&&space;0&space;&&space;1&space;-&space;\tau^{-1}dt\\&space;\end{bmatrix}&space;\begin{bmatrix}&space;y[k]&space;\\&space;\theta[k]&space;\\&space;\delta[k]&space;\end{bmatrix}&space;+&space;\begin{bmatrix}&space;0&space;\\&space;0&space;\\&space;\tau^{-1}dt&space;\\&space;\end{bmatrix}&space;\delta_{input}&space;-&space;\begin{bmatrix}&space;0&space;\\&space;-\frac{v&space;\delta_r&space;dt}{L&space;\&space;cos^{2}&space;\delta_r}&space;\\&space;0&space;\\&space;\end{bmatrix}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\boldsymbol{X}[k+1]&space;=&space;\begin{bmatrix}&space;1&space;&&space;vdt&space;&&space;0&space;\\&space;0&space;&&space;1&space;&&space;\frac{vdt}{L&space;\&space;cos^{2}&space;\delta_r}&space;\\&space;0&space;&&space;0&space;&&space;1&space;-&space;\tau^{-1}dt\\&space;\end{bmatrix}&space;\begin{bmatrix}&space;y[k]&space;\\&space;\theta[k]&space;\\&space;\delta[k]&space;\end{bmatrix}&space;+&space;\begin{bmatrix}&space;0&space;\\&space;0&space;\\&space;\tau^{-1}dt&space;\\&space;\end{bmatrix}&space;\delta_{input}&space;-&space;\begin{bmatrix}&space;0&space;\\&space;-\frac{v&space;\delta_r&space;dt}{L&space;\&space;cos^{2}&space;\delta_r}&space;\\&space;0&space;\\&space;\end{bmatrix}" title="\boldsymbol{X}[k+1] = \begin{bmatrix} 1 & vdt & 0 \\ 0 & 1 & \frac{vdt}{L \ cos^{2} \delta_r} \\ 0 & 0 & 1 - \tau^{-1}dt\\ \end{bmatrix} \begin{bmatrix} y[k] \\ \theta[k] \\ \delta[k] \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ \tau^{-1}dt \\ \end{bmatrix} \delta_{input} - \begin{bmatrix} 0 \\ -\frac{v \delta_r dt}{L \ cos^{2} \delta_r} \\ 0 \\ \end{bmatrix}" /></a>
|
||||||
|
|
||||||
|
and \delta_r denoted
|
||||||
|
|
||||||
|
<a href="https://www.codecogs.com/eqnedit.php?latex=\delta_r&space;=&space;\arctan(\frac{L}{R})" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\delta_r&space;=&space;\arctan(\frac{L}{R})" title="\delta_r = \arctan(\frac{L}{R})" /></a>
|
||||||
|
|
||||||
|
R is the curvatures of the reference trajectory.
|
||||||
|
|
||||||
|
Now we can get the linear state equation and can apply the MPC theory.
|
||||||
|
|
||||||
|
However, you should care that this state euation could be changed during the predict horizon.
|
||||||
|
I implemented this, so if you know about the detail please go to "IteraticeMPC_func.py"
|
||||||
|
|
||||||
|
# Expected Results
|
||||||
|
|
||||||
|
# Usage
|
||||||
|
|
||||||
|
```
|
||||||
|
$ python main_track.py
|
||||||
|
```
|
||||||
|
|
||||||
|
# Reference
|
||||||
|
- 1. https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B (Japanese)
|
||||||
|
|
@ -1,233 +0,0 @@
|
||||||
"""
|
|
||||||
Cubic spline planner
|
|
||||||
Author: Atsushi Sakai(@Atsushi_twi)
|
|
||||||
"""
|
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
import bisect
|
|
||||||
|
|
||||||
|
|
||||||
class Spline:
|
|
||||||
"""
|
|
||||||
Cubic Spline class
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, x, y):
|
|
||||||
self.b, self.c, self.d, self.w = [], [], [], []
|
|
||||||
|
|
||||||
self.x = x
|
|
||||||
self.y = y
|
|
||||||
|
|
||||||
self.nx = len(x) # dimension of x
|
|
||||||
h = np.diff(x)
|
|
||||||
|
|
||||||
# calc coefficient c
|
|
||||||
self.a = [iy for iy in y]
|
|
||||||
|
|
||||||
# calc coefficient c
|
|
||||||
A = self.__calc_A(h)
|
|
||||||
B = self.__calc_B(h)
|
|
||||||
self.c = np.linalg.solve(A, B)
|
|
||||||
# print(self.c1)
|
|
||||||
|
|
||||||
# calc spline coefficient b and d
|
|
||||||
for i in range(self.nx - 1):
|
|
||||||
self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
|
|
||||||
tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
|
|
||||||
(self.c[i + 1] + 2.0 * self.c[i]) / 3.0
|
|
||||||
self.b.append(tb)
|
|
||||||
|
|
||||||
def calc(self, t):
|
|
||||||
"""
|
|
||||||
Calc position
|
|
||||||
if t is outside of the input x, return None
|
|
||||||
"""
|
|
||||||
|
|
||||||
if t < self.x[0]:
|
|
||||||
return None
|
|
||||||
elif t > self.x[-1]:
|
|
||||||
return None
|
|
||||||
|
|
||||||
i = self.__search_index(t)
|
|
||||||
dx = t - self.x[i]
|
|
||||||
result = self.a[i] + self.b[i] * dx + \
|
|
||||||
self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
|
|
||||||
|
|
||||||
return result
|
|
||||||
|
|
||||||
def calcd(self, t):
|
|
||||||
"""
|
|
||||||
Calc first derivative
|
|
||||||
if t is outside of the input x, return None
|
|
||||||
"""
|
|
||||||
|
|
||||||
if t < self.x[0]:
|
|
||||||
return None
|
|
||||||
elif t > self.x[-1]:
|
|
||||||
return None
|
|
||||||
|
|
||||||
i = self.__search_index(t)
|
|
||||||
dx = t - self.x[i]
|
|
||||||
result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
|
|
||||||
return result
|
|
||||||
|
|
||||||
def calcdd(self, t):
|
|
||||||
"""
|
|
||||||
Calc second derivative
|
|
||||||
"""
|
|
||||||
|
|
||||||
if t < self.x[0]:
|
|
||||||
return None
|
|
||||||
elif t > self.x[-1]:
|
|
||||||
return None
|
|
||||||
|
|
||||||
i = self.__search_index(t)
|
|
||||||
dx = t - self.x[i]
|
|
||||||
result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
|
|
||||||
return result
|
|
||||||
|
|
||||||
def __search_index(self, x):
|
|
||||||
"""
|
|
||||||
search data segment index
|
|
||||||
"""
|
|
||||||
return bisect.bisect(self.x, x) - 1
|
|
||||||
|
|
||||||
def __calc_A(self, h):
|
|
||||||
"""
|
|
||||||
calc matrix A for spline coefficient c
|
|
||||||
"""
|
|
||||||
A = np.zeros((self.nx, self.nx))
|
|
||||||
A[0, 0] = 1.0
|
|
||||||
for i in range(self.nx - 1):
|
|
||||||
if i != (self.nx - 2):
|
|
||||||
A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
|
|
||||||
A[i + 1, i] = h[i]
|
|
||||||
A[i, i + 1] = h[i]
|
|
||||||
|
|
||||||
A[0, 1] = 0.0
|
|
||||||
A[self.nx - 1, self.nx - 2] = 0.0
|
|
||||||
A[self.nx - 1, self.nx - 1] = 1.0
|
|
||||||
# print(A)
|
|
||||||
return A
|
|
||||||
|
|
||||||
def __calc_B(self, h):
|
|
||||||
"""
|
|
||||||
calc matrix B for spline coefficient c
|
|
||||||
"""
|
|
||||||
B = np.zeros(self.nx)
|
|
||||||
for i in range(self.nx - 2):
|
|
||||||
B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
|
|
||||||
h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
|
|
||||||
return B
|
|
||||||
|
|
||||||
|
|
||||||
class Spline2D:
|
|
||||||
"""
|
|
||||||
2D Cubic Spline class
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, x, y):
|
|
||||||
self.s = self.__calc_s(x, y)
|
|
||||||
self.sx = Spline(self.s, x)
|
|
||||||
self.sy = Spline(self.s, y)
|
|
||||||
|
|
||||||
def __calc_s(self, x, y):
|
|
||||||
dx = np.diff(x)
|
|
||||||
dy = np.diff(y)
|
|
||||||
self.ds = [math.sqrt(idx ** 2 + idy ** 2)
|
|
||||||
for (idx, idy) in zip(dx, dy)]
|
|
||||||
s = [0]
|
|
||||||
s.extend(np.cumsum(self.ds))
|
|
||||||
return s
|
|
||||||
|
|
||||||
def calc_position(self, s):
|
|
||||||
"""
|
|
||||||
calc position
|
|
||||||
"""
|
|
||||||
x = self.sx.calc(s)
|
|
||||||
y = self.sy.calc(s)
|
|
||||||
|
|
||||||
return x, y
|
|
||||||
|
|
||||||
def calc_curvature(self, s):
|
|
||||||
"""
|
|
||||||
calc curvature
|
|
||||||
"""
|
|
||||||
dx = self.sx.calcd(s)
|
|
||||||
ddx = self.sx.calcdd(s)
|
|
||||||
dy = self.sy.calcd(s)
|
|
||||||
ddy = self.sy.calcdd(s)
|
|
||||||
k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
|
|
||||||
return k
|
|
||||||
|
|
||||||
def calc_yaw(self, s):
|
|
||||||
"""
|
|
||||||
calc yaw
|
|
||||||
"""
|
|
||||||
dx = self.sx.calcd(s)
|
|
||||||
dy = self.sy.calcd(s)
|
|
||||||
yaw = math.atan2(dy, dx)
|
|
||||||
return yaw
|
|
||||||
|
|
||||||
|
|
||||||
def calc_spline_course(x, y, ds=0.1):
|
|
||||||
sp = Spline2D(x, y)
|
|
||||||
s = list(np.arange(0, sp.s[-1], ds))
|
|
||||||
|
|
||||||
rx, ry, ryaw, rk = [], [], [], []
|
|
||||||
for i_s in s:
|
|
||||||
ix, iy = sp.calc_position(i_s)
|
|
||||||
rx.append(ix)
|
|
||||||
ry.append(iy)
|
|
||||||
ryaw.append(sp.calc_yaw(i_s))
|
|
||||||
rk.append(sp.calc_curvature(i_s))
|
|
||||||
|
|
||||||
return rx, ry, ryaw, rk, s
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
print("Spline 2D test")
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
|
|
||||||
y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
|
|
||||||
ds = 0.1 # [m] distance of each intepolated points
|
|
||||||
|
|
||||||
sp = Spline2D(x, y)
|
|
||||||
s = np.arange(0, sp.s[-1], ds)
|
|
||||||
|
|
||||||
rx, ry, ryaw, rk = [], [], [], []
|
|
||||||
for i_s in s:
|
|
||||||
ix, iy = sp.calc_position(i_s)
|
|
||||||
rx.append(ix)
|
|
||||||
ry.append(iy)
|
|
||||||
ryaw.append(sp.calc_yaw(i_s))
|
|
||||||
rk.append(sp.calc_curvature(i_s))
|
|
||||||
|
|
||||||
plt.subplots(1)
|
|
||||||
plt.plot(x, y, "xb", label="input")
|
|
||||||
plt.plot(rx, ry, "-r", label="spline")
|
|
||||||
plt.grid(True)
|
|
||||||
plt.axis("equal")
|
|
||||||
plt.xlabel("x[m]")
|
|
||||||
plt.ylabel("y[m]")
|
|
||||||
plt.legend()
|
|
||||||
|
|
||||||
plt.subplots(1)
|
|
||||||
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
|
|
||||||
plt.grid(True)
|
|
||||||
plt.legend()
|
|
||||||
plt.xlabel("line length[m]")
|
|
||||||
plt.ylabel("yaw angle[deg]")
|
|
||||||
|
|
||||||
plt.subplots(1)
|
|
||||||
plt.plot(s, rk, "-r", label="curvature")
|
|
||||||
plt.grid(True)
|
|
||||||
plt.legend()
|
|
||||||
plt.xlabel("line length[m]")
|
|
||||||
plt.ylabel("curvature [1/m]")
|
|
||||||
|
|
||||||
plt.show()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
|
|
@ -1,614 +0,0 @@
|
||||||
"""
|
|
||||||
Path tracking simulation with iterative linear model predictive control for speed and steer control
|
|
||||||
author: Atsushi Sakai (@Atsushi_twi)
|
|
||||||
"""
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import cvxpy
|
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
import sys
|
|
||||||
|
|
||||||
try:
|
|
||||||
import pathplanner
|
|
||||||
except:
|
|
||||||
raise
|
|
||||||
|
|
||||||
|
|
||||||
NX = 4 # x = x, y, v, yaw
|
|
||||||
NU = 2 # a = [accel, steer]
|
|
||||||
T = 5 # horizon length
|
|
||||||
|
|
||||||
# mpc parameters
|
|
||||||
R = np.diag([0.01, 0.01]) # input cost matrix
|
|
||||||
Rd = np.diag([0.01, 1.0]) # input difference cost matrix
|
|
||||||
Q = np.diag([1.0, 1.0, 0.5, 0.5]) # state cost matrix
|
|
||||||
Qf = Q # state final matrix
|
|
||||||
GOAL_DIS = 1.5 # goal distance
|
|
||||||
STOP_SPEED = 0.5 / 3.6 # stop speed
|
|
||||||
MAX_TIME = 500.0 # max simulation time
|
|
||||||
|
|
||||||
# iterative paramter
|
|
||||||
MAX_ITER = 3 # Max iteration
|
|
||||||
DU_TH = 0.1 # iteration finish param
|
|
||||||
|
|
||||||
TARGET_SPEED = 10.0 / 3.6 # [m/s] target speed
|
|
||||||
N_IND_SEARCH = 10 # Search index number
|
|
||||||
|
|
||||||
DT = 0.2 # [s] time tick
|
|
||||||
|
|
||||||
# Vehicle parameters
|
|
||||||
LENGTH = 4.5 # [m]
|
|
||||||
WIDTH = 2.0 # [m]
|
|
||||||
BACKTOWHEEL = 1.0 # [m]
|
|
||||||
WHEEL_LEN = 0.3 # [m]
|
|
||||||
WHEEL_WIDTH = 0.2 # [m]
|
|
||||||
TREAD = 0.7 # [m]
|
|
||||||
WB = 2.5 # [m]
|
|
||||||
|
|
||||||
MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad]
|
|
||||||
MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s]
|
|
||||||
MAX_SPEED = 55.0 / 3.6 # maximum speed [m/s]
|
|
||||||
MIN_SPEED = -20.0 / 3.6 # minimum speed [m/s]
|
|
||||||
MAX_ACCEL = 1.0 # maximum accel [m/ss]
|
|
||||||
|
|
||||||
show_animation = True
|
|
||||||
|
|
||||||
|
|
||||||
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
|
|
||||||
self.predelta = None
|
|
||||||
|
|
||||||
|
|
||||||
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.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) / WB
|
|
||||||
|
|
||||||
B = np.zeros((NX, NU))
|
|
||||||
B[2, 0] = DT
|
|
||||||
B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)
|
|
||||||
|
|
||||||
C = np.zeros(NX)
|
|
||||||
C[0] = DT * v * math.sin(phi) * phi
|
|
||||||
C[1] = - DT * v * math.cos(phi) * phi
|
|
||||||
C[3] = - v * delta / (WB * math.cos(delta) ** 2)
|
|
||||||
|
|
||||||
return A, B, C
|
|
||||||
|
|
||||||
|
|
||||||
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover
|
|
||||||
|
|
||||||
outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
|
|
||||||
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
|
|
||||||
|
|
||||||
fr_wheel = np.array([[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.array([[math.cos(yaw), math.sin(yaw)],
|
|
||||||
[-math.sin(yaw), math.cos(yaw)]])
|
|
||||||
Rot2 = np.array([[math.cos(steer), math.sin(steer)],
|
|
||||||
[-math.sin(steer), math.cos(steer)]])
|
|
||||||
|
|
||||||
fr_wheel = (fr_wheel.T.dot(Rot2)).T
|
|
||||||
fl_wheel = (fl_wheel.T.dot(Rot2)).T
|
|
||||||
fr_wheel[0, :] += WB
|
|
||||||
fl_wheel[0, :] += WB
|
|
||||||
|
|
||||||
fr_wheel = (fr_wheel.T.dot(Rot1)).T
|
|
||||||
fl_wheel = (fl_wheel.T.dot(Rot1)).T
|
|
||||||
|
|
||||||
outline = (outline.T.dot(Rot1)).T
|
|
||||||
rr_wheel = (rr_wheel.T.dot(Rot1)).T
|
|
||||||
rl_wheel = (rl_wheel.T.dot(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 >= MAX_STEER:
|
|
||||||
delta = MAX_STEER
|
|
||||||
elif delta <= -MAX_STEER:
|
|
||||||
delta = -MAX_STEER
|
|
||||||
|
|
||||||
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 > MAX_SPEED:
|
|
||||||
state.v = MAX_SPEED
|
|
||||||
elif state. v < MIN_SPEED:
|
|
||||||
state.v = MIN_SPEED
|
|
||||||
|
|
||||||
return state
|
|
||||||
|
|
||||||
|
|
||||||
def get_nparray_from_matrix(x):
|
|
||||||
return np.array(x).flatten()
|
|
||||||
|
|
||||||
|
|
||||||
def calc_nearest_index(state, cx, cy, cyaw, pind):
|
|
||||||
|
|
||||||
dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]]
|
|
||||||
dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]]
|
|
||||||
|
|
||||||
d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
|
|
||||||
|
|
||||||
mind = min(d)
|
|
||||||
|
|
||||||
ind = d.index(mind) + pind
|
|
||||||
|
|
||||||
mind = math.sqrt(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 enumerate(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(MAX_ITER):
|
|
||||||
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]) <=
|
|
||||||
MAX_DSTEER * DT]
|
|
||||||
|
|
||||||
cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
|
|
||||||
|
|
||||||
constraints += [x[:, 0] == x0]
|
|
||||||
constraints += [x[2, :] <= MAX_SPEED]
|
|
||||||
constraints += [x[2, :] >= MIN_SPEED]
|
|
||||||
constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL]
|
|
||||||
constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
|
|
||||||
|
|
||||||
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
|
|
||||||
prob.solve(solver=cvxpy.ECOS, 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, pind)
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
isgoal = (d <= GOAL_DIS)
|
|
||||||
|
|
||||||
if abs(tind - nind) >= 5:
|
|
||||||
isgoal = False
|
|
||||||
|
|
||||||
isstop = (abs(state.v) <= STOP_SPEED)
|
|
||||||
|
|
||||||
if isgoal and isstop:
|
|
||||||
return True
|
|
||||||
|
|
||||||
return False
|
|
||||||
|
|
||||||
|
|
||||||
def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
|
|
||||||
"""
|
|
||||||
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 = initial_state
|
|
||||||
|
|
||||||
# initial yaw compensation
|
|
||||||
if state.yaw - cyaw[0] >= math.pi:
|
|
||||||
state.yaw -= math.pi * 2.0
|
|
||||||
elif state.yaw - cyaw[0] <= -math.pi:
|
|
||||||
state.yaw += math.pi * 2.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, 0)
|
|
||||||
|
|
||||||
odelta, oa = None, None
|
|
||||||
|
|
||||||
cyaw = smooth_yaw(cyaw)
|
|
||||||
|
|
||||||
while MAX_TIME >= 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: # pragma: no cover
|
|
||||||
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)
|
|
||||||
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_straight_course(dl):
|
|
||||||
ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
|
|
||||||
ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|
||||||
cx, cy, cyaw, ck, s = pathplanner.calc_spline_course(
|
|
||||||
ax, ay, ds=dl)
|
|
||||||
|
|
||||||
return cx, cy, cyaw, ck
|
|
||||||
|
|
||||||
|
|
||||||
def get_straight_course2(dl):
|
|
||||||
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
|
|
||||||
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
|
|
||||||
cx, cy, cyaw, ck, s = pathplanner.calc_spline_course(
|
|
||||||
ax, ay, ds=dl)
|
|
||||||
|
|
||||||
return cx, cy, cyaw, ck
|
|
||||||
|
|
||||||
|
|
||||||
def get_straight_course3(dl):
|
|
||||||
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
|
|
||||||
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
|
|
||||||
cx, cy, cyaw, ck, s = pathplanner.calc_spline_course(
|
|
||||||
ax, ay, ds=dl)
|
|
||||||
|
|
||||||
cyaw = [i - math.pi for i in cyaw]
|
|
||||||
|
|
||||||
return cx, cy, cyaw, ck
|
|
||||||
|
|
||||||
|
|
||||||
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 = pathplanner.calc_spline_course(
|
|
||||||
ax, ay, ds=dl)
|
|
||||||
|
|
||||||
return cx, cy, cyaw, ck
|
|
||||||
|
|
||||||
|
|
||||||
def get_switch_back_course(dl):
|
|
||||||
ax = [0.0, 30.0, 6.0, 20.0, 35.0]
|
|
||||||
ay = [0.0, 0.0, 20.0, 35.0, 20.0]
|
|
||||||
cx, cy, cyaw, ck, s = pathplanner.calc_spline_course(
|
|
||||||
ax, ay, ds=dl)
|
|
||||||
ax = [35.0, 10.0, 0.0, 0.0]
|
|
||||||
ay = [20.0, 30.0, 5.0, 0.0]
|
|
||||||
cx2, cy2, cyaw2, ck2, s2 = pathplanner.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_straight_course(dl)
|
|
||||||
# cx, cy, cyaw, ck = get_straight_course2(dl)
|
|
||||||
cx, cy, cyaw, ck = get_straight_course3(dl)
|
|
||||||
# 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)
|
|
||||||
|
|
||||||
initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
|
|
||||||
|
|
||||||
t, x, y, yaw, v, d, a = do_simulation(
|
|
||||||
cx, cy, cyaw, ck, sp, dl, initial_state)
|
|
||||||
|
|
||||||
if show_animation: # pragma: no cover
|
|
||||||
plt.close("all")
|
|
||||||
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()
|
|
||||||
|
|
||||||
|
|
||||||
def main2():
|
|
||||||
print(__file__ + " start!!")
|
|
||||||
|
|
||||||
dl = 1.0 # course tick
|
|
||||||
cx, cy, cyaw, ck = get_straight_course3(dl)
|
|
||||||
|
|
||||||
sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
|
|
||||||
|
|
||||||
initial_state = State(x=cx[0], y=cy[0], yaw=0.0, v=0.0)
|
|
||||||
|
|
||||||
t, x, y, yaw, v, d, a = do_simulation(
|
|
||||||
cx, cy, cyaw, ck, sp, dl, initial_state)
|
|
||||||
|
|
||||||
if show_animation: # pragma: no cover
|
|
||||||
plt.close("all")
|
|
||||||
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()
|
|
||||||
main2()
|
|
||||||
Loading…
Reference in New Issue