Merge branch 'master' of github.com:Shunichi09/nonlinear_control
This commit is contained in:
Shunichi09
commit
362e44fbf3
|
|
@ -8,6 +8,7 @@ Now you can see the examples of control theories as following
|
|||
|
||||
- **Nonlinear Model Predictive Control with CGMRES**
|
||||
- **Linear Model Predictive Control**(as generic function such as matlab tool box)
|
||||
- **Extended Linear Model Predictive Control for vehicle model**
|
||||
|
||||
# Usage and Details
|
||||
you can see the usage in each folder
|
||||
|
|
|
|||
|
|
@ -0,0 +1,41 @@
|
|||
# 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
|
||||
|
||||
<img src = https://github.com/Shunichi09/linear_nonlinear_control/blob/demo_gif/mpc/extend/animation_all.gif width = 70%>
|
||||
|
||||
<img src = https://github.com/Shunichi09/linear_nonlinear_control/blob/demo_gif/mpc/extend/animation_zoom.gif width = 70%>
|
||||
|
||||
# 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)
|
||||
|
|
@ -84,6 +84,42 @@ def square_make_with_angles(center_x, center_y, size, angle):
|
|||
return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys)
|
||||
|
||||
|
||||
def circle_make_with_angles(center_x, center_y, radius, angle):
|
||||
'''
|
||||
Create circle matrix with angle line matrix
|
||||
|
||||
Parameters
|
||||
-------
|
||||
center_x : float
|
||||
the center x position of the circle
|
||||
center_y : float
|
||||
the center y position of the circle
|
||||
radius : float
|
||||
angle : float [rad]
|
||||
|
||||
Returns
|
||||
-------
|
||||
circle xs : numpy.ndarray
|
||||
circle ys : numpy.ndarray
|
||||
angle line xs : numpy.ndarray
|
||||
angle line ys : numpy.ndarray
|
||||
'''
|
||||
|
||||
point_num = 100 # 分解能
|
||||
|
||||
circle_xs = []
|
||||
circle_ys = []
|
||||
|
||||
for i in range(point_num + 1):
|
||||
circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num))
|
||||
circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num))
|
||||
|
||||
angle_line_xs = [center_x, center_x + math.cos(angle) * radius]
|
||||
angle_line_ys = [center_y, center_y + math.sin(angle) * radius]
|
||||
|
||||
return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys)
|
||||
|
||||
|
||||
class AnimDrawer():
|
||||
"""create animation of path and robot
|
||||
|
||||
|
|
@ -99,22 +135,29 @@ class AnimDrawer():
|
|||
Parameters
|
||||
------------
|
||||
objects : list of objects
|
||||
|
||||
Notes
|
||||
---------
|
||||
lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref
|
||||
"""
|
||||
self.lead_car_history_state = objects[0]
|
||||
self.follow_car_history_state = objects[1]
|
||||
self.lead_car_history_predict_state = objects[1]
|
||||
self.traj = objects[2]
|
||||
self.history_traj_ref = objects[3]
|
||||
self.history_angle_ref = objects[4]
|
||||
|
||||
self.history_xs = [self.lead_car_history_state[:, 0], self.follow_car_history_state[:, 0]]
|
||||
self.history_ys = [self.lead_car_history_state[:, 1], self.follow_car_history_state[:, 1]]
|
||||
self.history_ths = [self.lead_car_history_state[:, 2], self.follow_car_history_state[:, 2]]
|
||||
self.history_xs = [self.lead_car_history_state[:, 0]]
|
||||
self.history_ys = [self.lead_car_history_state[:, 1]]
|
||||
self.history_ths = [self.lead_car_history_state[:, 2]]
|
||||
|
||||
# setting up figure
|
||||
self.anim_fig = plt.figure(dpi=150)
|
||||
self.axis = self.anim_fig.add_subplot(111)
|
||||
|
||||
# imgs
|
||||
self.object_imgs = []
|
||||
self.car_imgs = []
|
||||
self.traj_imgs = []
|
||||
self.predict_imgs = []
|
||||
|
||||
def draw_anim(self, interval=50):
|
||||
"""draw the animation and save
|
||||
|
|
@ -153,9 +196,11 @@ class AnimDrawer():
|
|||
self.axis.set_ylabel(r'$\it{y}$ [m]')
|
||||
self.axis.set_aspect('equal', adjustable='box')
|
||||
|
||||
# (2) set the xlim and ylim
|
||||
self.axis.set_xlim(-2, 20)
|
||||
self.axis.set_ylim(-10, 10)
|
||||
LOW_MARGIN = 5
|
||||
HIGH_MARGIN = 5
|
||||
|
||||
self.axis.set_xlim(np.min(self.history_xs) - LOW_MARGIN, np.max(self.history_xs) + HIGH_MARGIN)
|
||||
self.axis.set_ylim(np.min(self.history_ys) - LOW_MARGIN, np.max(self.history_ys) + HIGH_MARGIN)
|
||||
|
||||
def _set_img(self):
|
||||
""" initialize the imgs of animation
|
||||
|
|
@ -167,14 +212,22 @@ class AnimDrawer():
|
|||
|
||||
for i in range(len(obj_color_list)):
|
||||
temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i])
|
||||
self.object_imgs.append(temp_img)
|
||||
self.car_imgs.append(temp_img)
|
||||
|
||||
traj_color_list = ["k", "m", "b"]
|
||||
traj_color_list = ["k", "b"]
|
||||
|
||||
for i in range(len(traj_color_list)):
|
||||
temp_img, = self.axis.plot([],[], color=traj_color_list[i], linestyle="dashed")
|
||||
self.traj_imgs.append(temp_img)
|
||||
|
||||
temp_img, = self.axis.plot([],[], ".", color="m")
|
||||
self.traj_imgs.append(temp_img)
|
||||
|
||||
# predict
|
||||
for _ in range(2 * len(self.history_angle_ref[0])):
|
||||
temp_img, = self.axis.plot([],[], color="g", linewidth=0.5) # point
|
||||
# temp_img, = self.axis.plot([],[], ".", color="g", linewidth=0.5) # point
|
||||
self.predict_imgs.append(temp_img)
|
||||
|
||||
def _update_anim(self, i):
|
||||
"""the update animation
|
||||
|
|
@ -193,12 +246,24 @@ class AnimDrawer():
|
|||
"""
|
||||
i = int(i * self.skip_num)
|
||||
|
||||
self._draw_objects(i)
|
||||
# self._draw_set_axis(i)
|
||||
self._draw_car(i)
|
||||
self._draw_traj(i)
|
||||
# self._draw_prediction(i)
|
||||
|
||||
return self.object_imgs, self.traj_imgs,
|
||||
return self.car_imgs, self.traj_imgs, self.predict_imgs,
|
||||
|
||||
def _draw_objects(self, i):
|
||||
def _draw_set_axis(self, i):
|
||||
"""
|
||||
"""
|
||||
# (2) set the xlim and ylim
|
||||
LOW_MARGIN = 20
|
||||
HIGH_MARGIN = 20
|
||||
OVER_LOOK = 50
|
||||
self.axis.set_xlim(np.min(self.history_xs[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_xs[0][i : i + OVER_LOOK]) + HIGH_MARGIN)
|
||||
self.axis.set_ylim(np.min(self.history_ys[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_ys[0][i : i + OVER_LOOK]) + HIGH_MARGIN)
|
||||
|
||||
def _draw_car(self, i):
|
||||
"""
|
||||
This private function is just divided thing of
|
||||
the _update_anim to see the code more clear
|
||||
|
|
@ -210,15 +275,13 @@ class AnimDrawer():
|
|||
the sampling time should be related to the sampling time of system
|
||||
"""
|
||||
# cars
|
||||
for j in range(2):
|
||||
fix_j = j * 2
|
||||
object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[j][i],
|
||||
self.history_ys[j][i],
|
||||
1.0,
|
||||
self.history_ths[j][i])
|
||||
object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[0][i],
|
||||
self.history_ys[0][i],
|
||||
5.0,
|
||||
self.history_ths[0][i])
|
||||
|
||||
self.object_imgs[fix_j].set_data([object_x, object_y])
|
||||
self.object_imgs[fix_j + 1].set_data([angle_x, angle_y])
|
||||
self.car_imgs[0].set_data([object_x, object_y])
|
||||
self.car_imgs[1].set_data([angle_x, angle_y])
|
||||
|
||||
def _draw_traj(self, i):
|
||||
"""
|
||||
|
|
@ -231,7 +294,31 @@ class AnimDrawer():
|
|||
time step of the animation
|
||||
the sampling time should be related to the sampling time of system
|
||||
"""
|
||||
for j in range(2):
|
||||
self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i])
|
||||
# car
|
||||
self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i])
|
||||
|
||||
self.traj_imgs[2].set_data(self.traj[0, :], self.traj[1, :])
|
||||
# all traj_ref
|
||||
self.traj_imgs[1].set_data(self.traj[0, :], self.traj[1, :])
|
||||
|
||||
# traj_ref
|
||||
# self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :])
|
||||
|
||||
def _draw_prediction(self, i):
|
||||
"""draw prediction
|
||||
|
||||
Parameters
|
||||
------------
|
||||
i : int
|
||||
time step of the animation
|
||||
the sampling time should be related to the sampling time of system
|
||||
"""
|
||||
|
||||
for j in range(0, len(self.history_angle_ref[0]), 4):
|
||||
fix_j = j * 2
|
||||
object_x, object_y, angle_x, angle_y =\
|
||||
circle_make_with_angles(self.lead_car_history_predict_state[i][j, 0],
|
||||
self.lead_car_history_predict_state[i][j, 1], 1.,
|
||||
self.lead_car_history_predict_state[i][j, 2])
|
||||
|
||||
self.predict_imgs[fix_j].set_data(object_x, object_y)
|
||||
self.predict_imgs[fix_j + 1].set_data(angle_x, angle_y)
|
||||
|
|
@ -11,11 +11,11 @@ class IterativeMpcController():
|
|||
"""
|
||||
Attributes
|
||||
------------
|
||||
A : numpy.ndarray
|
||||
Ad_s : list of numpy.ndarray
|
||||
system matrix
|
||||
B : numpy.ndarray
|
||||
Bd_s : list of numpy.ndarray
|
||||
input matrix
|
||||
W_D : numpy.ndarray
|
||||
W_D_s : list of numpy.ndarray
|
||||
distubance matrix in state equation
|
||||
Q : numpy.ndarray
|
||||
evaluation function weight for states
|
||||
|
|
@ -107,7 +107,7 @@ class IterativeMpcController():
|
|||
|
||||
A_factorials.append(temp_mat) # after we use this factorials
|
||||
|
||||
print("phi_mat = \n{0}".format(self.phi_mat))
|
||||
# print("phi_mat = \n{0}".format(self.phi_mat))
|
||||
|
||||
self.gamma_mat = copy.deepcopy(self.Bd_s[0])
|
||||
gammma_mat_temp = copy.deepcopy(self.Bd_s[0])
|
||||
|
|
@ -117,7 +117,7 @@ class IterativeMpcController():
|
|||
gammma_mat_temp = temp_1_mat + gammma_mat_temp
|
||||
self.gamma_mat = np.vstack((self.gamma_mat, gammma_mat_temp))
|
||||
|
||||
print("gamma_mat = \n{0}".format(self.gamma_mat))
|
||||
# print("gamma_mat = \n{0}".format(self.gamma_mat))
|
||||
|
||||
self.theta_mat = copy.deepcopy(self.gamma_mat)
|
||||
|
||||
|
|
@ -127,25 +127,25 @@ class IterativeMpcController():
|
|||
|
||||
self.theta_mat = np.hstack((self.theta_mat, temp_mat))
|
||||
|
||||
print("theta_mat = \n{0}".format(self.theta_mat))
|
||||
# print("theta_mat = \n{0}".format(self.theta_mat))
|
||||
|
||||
# disturbance
|
||||
print("A_factorials_mat = \n{0}".format(A_factorials))
|
||||
# print("A_factorials_mat = \n{0}".format(A_factorials))
|
||||
A_factorials_mat = np.array(A_factorials).reshape(-1, self.state_size)
|
||||
print("A_factorials_mat = \n{0}".format(A_factorials_mat))
|
||||
# print("A_factorials_mat = \n{0}".format(A_factorials_mat))
|
||||
|
||||
eye = np.eye(self.state_size)
|
||||
self.dist_mat = np.vstack((eye, A_factorials_mat[:-self.state_size, :]))
|
||||
base_mat = copy.deepcopy(self.dist_mat)
|
||||
|
||||
print("base_mat = \n{0}".format(base_mat))
|
||||
# print("base_mat = \n{0}".format(base_mat))
|
||||
|
||||
for i in range(self.pre_step - 1):
|
||||
temp_mat = np.zeros_like(A_factorials_mat)
|
||||
temp_mat[int((i + 1)*self.state_size): , :] = base_mat[:-int((i + 1)*self.state_size) , :]
|
||||
self.dist_mat = np.hstack((self.dist_mat, temp_mat))
|
||||
|
||||
print("dist_mat = \n{0}".format(self.dist_mat))
|
||||
# print("dist_mat = \n{0}".format(self.dist_mat))
|
||||
|
||||
W_Ds = copy.deepcopy(self.W_D_s[0])
|
||||
|
||||
|
|
@ -154,7 +154,7 @@ class IterativeMpcController():
|
|||
|
||||
self.dist_mat = np.dot(self.dist_mat, W_Ds)
|
||||
|
||||
print("dist_mat = \n{0}".format(self.dist_mat))
|
||||
# print("dist_mat = \n{0}".format(self.dist_mat))
|
||||
|
||||
# evaluation function weight
|
||||
diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)])
|
||||
|
|
@ -163,8 +163,8 @@ class IterativeMpcController():
|
|||
self.Qs = np.diag(diag_Qs.flatten())
|
||||
self.Rs = np.diag(diag_Rs.flatten())
|
||||
|
||||
print("Qs = \n{0}".format(self.Qs))
|
||||
print("Rs = \n{0}".format(self.Rs))
|
||||
# print("Qs = \n{0}".format(self.Qs))
|
||||
# print("Rs = \n{0}".format(self.Rs))
|
||||
|
||||
# constraints
|
||||
# about dt U
|
||||
|
|
@ -175,7 +175,7 @@ class IterativeMpcController():
|
|||
self.F[i * 2: (i + 1) * 2, i] = np.array([1., -1.])
|
||||
temp_F = copy.deepcopy(self.F)
|
||||
|
||||
print("F = \n{0}".format(self.F))
|
||||
# print("F = \n{0}".format(self.F))
|
||||
|
||||
for i in range(self.pre_step - 1):
|
||||
temp_F = copy.deepcopy(temp_F)
|
||||
|
|
@ -195,9 +195,9 @@ class IterativeMpcController():
|
|||
|
||||
self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten()
|
||||
|
||||
print("F = \n{0}".format(self.F))
|
||||
print("F1 = \n{0}".format(self.F1))
|
||||
print("f = \n{0}".format(self.f))
|
||||
# print("F = \n{0}".format(self.F))
|
||||
# print("F1 = \n{0}".format(self.F1))
|
||||
# print("f = \n{0}".format(self.f))
|
||||
|
||||
# about dt_u
|
||||
if self.dt_input_lower is not None:
|
||||
|
|
@ -217,8 +217,8 @@ class IterativeMpcController():
|
|||
|
||||
self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten()
|
||||
|
||||
print("W = \n{0}".format(self.W))
|
||||
print("omega = \n{0}".format(self.omega))
|
||||
# print("W = \n{0}".format(self.W))
|
||||
# print("omega = \n{0}".format(self.omega))
|
||||
|
||||
# about state
|
||||
print("check the matrix!! if you think rite, plese push enter")
|
||||
|
|
@ -235,8 +235,9 @@ class IterativeMpcController():
|
|||
|
||||
References
|
||||
------------
|
||||
opt_input : numpy.ndarray, shape(input_length, )
|
||||
opt_u : numpy.ndarray, shape(input_length, )
|
||||
optimal input
|
||||
all_opt_u : numpy.ndarray, shape(PREDICT_STEP, input_length)
|
||||
"""
|
||||
temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1))
|
||||
temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1))
|
||||
|
|
@ -278,10 +279,18 @@ class IterativeMpcController():
|
|||
|
||||
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
|
||||
|
||||
# calc all predit u
|
||||
all_opt_u = [copy.deepcopy(opt_u)]
|
||||
temp_u = copy.deepcopy(opt_u)
|
||||
|
||||
for i in range(1, self.pre_step):
|
||||
temp_u += opt_dt_us[i * self.input_size: (i + 1) * self.input_size]
|
||||
all_opt_u.append(copy.deepcopy(temp_u))
|
||||
|
||||
# save
|
||||
self.history_us.append(opt_u)
|
||||
|
||||
return opt_u
|
||||
return opt_u, np.array(all_opt_u)
|
||||
|
||||
def update_system_model(self, system_model):
|
||||
"""update system model
|
||||
|
|
@ -138,6 +138,22 @@ def calc_curvatures(traj_ref, predict_step, num_skip):
|
|||
|
||||
return angles, curvatures
|
||||
|
||||
def calc_ideal_vel(traj_ref, dt):
|
||||
"""
|
||||
Parameters
|
||||
------------
|
||||
traj_ref : numpy.ndarray, shape (2, N)
|
||||
these points should follow subseqently
|
||||
dt : float
|
||||
sampling time of system
|
||||
"""
|
||||
# end point and start point
|
||||
diff = traj_ref[:, -1] - traj_ref[:, 0]
|
||||
distance = np.sqrt(np.sum(diff**2))
|
||||
|
||||
V = distance / (dt * traj_ref.shape[1])
|
||||
|
||||
return V
|
||||
|
||||
def main():
|
||||
"""
|
||||
|
|
@ -4,12 +4,12 @@ import math
|
|||
import copy
|
||||
|
||||
# from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
|
||||
from iterative_MPC import IterativeMpcController
|
||||
from extended_MPC import IterativeMpcController
|
||||
from animation import AnimDrawer
|
||||
# from control import matlab
|
||||
from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position
|
||||
from traj_func import make_sample_traj
|
||||
from func_curvature import calc_curvatures
|
||||
from func_curvature import calc_curvatures, calc_ideal_vel
|
||||
|
||||
class WheeledSystem():
|
||||
"""SampleSystem, this is the simulator
|
||||
|
|
@ -21,6 +21,11 @@ class WheeledSystem():
|
|||
system states, [x, y, phi, beta]
|
||||
history_xs : list
|
||||
time history of state
|
||||
tau : float
|
||||
time constant of tire
|
||||
FRONT_WHEEL_BASE : float
|
||||
REAR_WHEEL_BASE : float
|
||||
predict_xs :
|
||||
"""
|
||||
def __init__(self, init_states=None):
|
||||
"""
|
||||
|
|
@ -41,6 +46,7 @@ class WheeledSystem():
|
|||
self.xs = copy.deepcopy(init_states)
|
||||
|
||||
self.history_xs = [init_states]
|
||||
self.history_predict_xs = []
|
||||
|
||||
def update_state(self, us, dt=0.01):
|
||||
"""
|
||||
|
|
@ -51,7 +57,6 @@ class WheeledSystem():
|
|||
dt : float in seconds, optional
|
||||
sampling time of simulation, default is 0.01 [s]
|
||||
"""
|
||||
# for theta 1, theta 1 dot, theta 2, theta 2 dot
|
||||
k0 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
k1 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
k2 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
|
|
@ -80,7 +85,50 @@ class WheeledSystem():
|
|||
# save
|
||||
save_states = copy.deepcopy(self.xs)
|
||||
self.history_xs.append(save_states)
|
||||
print(self.xs)
|
||||
# print(self.xs)
|
||||
|
||||
def predict_state(self, us, dt=0.01):
|
||||
"""make predict state by using optimal input made by MPC
|
||||
Paramaters
|
||||
-----------
|
||||
us : array-like, shape(2, N)
|
||||
optimal input made by MPC
|
||||
dt : float in seconds, optional
|
||||
sampling time of simulation, default is 0.01 [s]
|
||||
"""
|
||||
|
||||
xs = copy.deepcopy(self.xs)
|
||||
predict_xs = [copy.deepcopy(xs)]
|
||||
|
||||
for i in range(us.shape[1]):
|
||||
k0 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
k1 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
k2 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
k3 = [0.0 for _ in range(self.NUM_STATE)]
|
||||
|
||||
functions = [self._func_x_1, self._func_x_2, self._func_x_3, self._func_x_4]
|
||||
|
||||
# solve Runge-Kutta
|
||||
for i, func in enumerate(functions):
|
||||
k0[i] = dt * func(xs[0], xs[1], xs[2], xs[3], us[0, i], us[1, i])
|
||||
|
||||
for i, func in enumerate(functions):
|
||||
k1[i] = dt * func(xs[0] + k0[0]/2., xs[1] + k0[1]/2., xs[2] + k0[2]/2., xs[3] + k0[3]/2., us[0, i], us[1, i])
|
||||
|
||||
for i, func in enumerate(functions):
|
||||
k2[i] = dt * func(xs[0] + k1[0]/2., xs[1] + k1[1]/2., xs[2] + k1[2]/2., xs[3] + k1[3]/2., us[0, i], us[1, i])
|
||||
|
||||
for i, func in enumerate(functions):
|
||||
k3[i] = dt * func(xs[0] + k2[0], xs[1] + k2[1], xs[2] + k2[2], xs[3] + k2[3], us[0, i], us[1, i])
|
||||
|
||||
xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6.
|
||||
xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
|
||||
xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
|
||||
xs[3] += (k0[3] + 2. * k1[3] + 2. * k2[3] + k3[3]) / 6.
|
||||
|
||||
predict_xs.append(copy.deepcopy(xs))
|
||||
|
||||
self.history_predict_xs.append(np.array(predict_xs))
|
||||
|
||||
def _func_x_1(self, y_1, y_2, y_3, y_4, u_1, u_2):
|
||||
"""
|
||||
|
|
@ -184,6 +232,7 @@ class SystemModel():
|
|||
"""
|
||||
calc next predict systemo models
|
||||
V : float
|
||||
current speed of car
|
||||
curvatures : list
|
||||
this is the next curvature's list
|
||||
predict_step : int
|
||||
|
|
@ -236,20 +285,19 @@ def main():
|
|||
# parameters
|
||||
dt = 0.01
|
||||
simulation_time = 20 # in seconds
|
||||
PREDICT_STEP = 15
|
||||
PREDICT_STEP = 30
|
||||
iteration_num = int(simulation_time / dt)
|
||||
|
||||
# make simulator with coninuous matrix
|
||||
init_xs_lead = np.array([0., 0., math.pi/4, 0.])
|
||||
init_xs_follow = np.array([0., 0., math.pi/4, 0.])
|
||||
init_xs_lead = np.array([0., 0., math.pi/6, 0.])
|
||||
lead_car = WheeledSystem(init_states=init_xs_lead)
|
||||
follow_car = WheeledSystem(init_states=init_xs_follow)
|
||||
|
||||
# make system model
|
||||
lead_car_system_model = SystemModel()
|
||||
follow_car_system_model = SystemModel()
|
||||
|
||||
# reference
|
||||
history_traj_ref = []
|
||||
history_angle_ref = []
|
||||
traj_ref_xs, traj_ref_ys = make_sample_traj(int(simulation_time/dt))
|
||||
traj_ref = np.array([traj_ref_xs, traj_ref_ys])
|
||||
|
||||
|
|
@ -257,31 +305,33 @@ def main():
|
|||
index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1))
|
||||
|
||||
# get traj's curvature
|
||||
NUM_SKIP = 5
|
||||
MARGIN = 10
|
||||
NUM_SKIP = 3
|
||||
MARGIN = 20
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
|
||||
|
||||
# save traj ref
|
||||
history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN])
|
||||
history_angle_ref.append(angles)
|
||||
|
||||
# print(history_traj_ref)
|
||||
# input()
|
||||
|
||||
# evaluation function weight
|
||||
Q = np.diag([100., 1., 1.])
|
||||
R = np.diag([0.01])
|
||||
Q = np.diag([1000000., 10., 1.])
|
||||
R = np.diag([0.1])
|
||||
|
||||
# System model update
|
||||
V = 3.0 # in pratical we should calc from the state
|
||||
V = calc_ideal_vel(traj_ref, dt) # in pratical we should calc from the state
|
||||
lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
|
||||
follow_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
|
||||
|
||||
# make controller with discreted matrix
|
||||
lead_controller = IterativeMpcController(lead_car_system_model, Q, R, PREDICT_STEP,
|
||||
dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
|
||||
input_upper=np.array([30.]), input_lower=np.array([-30.]))
|
||||
dt_input_upper=np.array([1 * dt]), dt_input_lower=np.array([-1 * dt]),
|
||||
input_upper=np.array([1.]), input_lower=np.array([-1.]))
|
||||
|
||||
follow_controller = IterativeMpcController(follow_car_system_model, Q, R, PREDICT_STEP,
|
||||
dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
|
||||
input_upper=np.array([30.]), input_lower=np.array([-30.]))
|
||||
|
||||
# initialize
|
||||
lead_controller.initialize_controller()
|
||||
follow_controller.initialize_controller()
|
||||
|
||||
for i in range(iteration_num):
|
||||
print("simulation time = {0}".format(i))
|
||||
|
|
@ -292,6 +342,7 @@ def main():
|
|||
|
||||
# nearest point
|
||||
index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1))
|
||||
|
||||
# end check
|
||||
if len(traj_ref_ys) <= index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN:
|
||||
print("break")
|
||||
|
|
@ -300,8 +351,12 @@ def main():
|
|||
# get traj's curvature
|
||||
angles, curvatures = calc_curvatures(traj_ref[:, index_min+MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
|
||||
|
||||
# save
|
||||
history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN])
|
||||
history_angle_ref.append(angles)
|
||||
|
||||
# System model update
|
||||
V = 4.0 # in pratical we should calc from the state
|
||||
V = calc_ideal_vel(traj_ref, dt) # in pratical we should calc from the state
|
||||
lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
|
||||
|
||||
# transformation
|
||||
|
|
@ -318,7 +373,7 @@ def main():
|
|||
relative_ref_angle = np.array(angles) - angles[0]
|
||||
|
||||
# make ref
|
||||
lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[i], 0.] for i in range(PREDICT_STEP)]).flatten()
|
||||
lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[-1], 0.] for i in range(PREDICT_STEP)]).flatten()
|
||||
|
||||
print("relative car state = {}".format(relative_car_state))
|
||||
print("nearest point = {}".format(nearest_point))
|
||||
|
|
@ -327,19 +382,27 @@ def main():
|
|||
# update system matrix
|
||||
lead_controller.update_system_model(lead_car_system_model)
|
||||
|
||||
lead_opt_u = lead_controller.calc_input(relative_car_state, lead_reference)
|
||||
lead_opt_u, all_opt_u = lead_controller.calc_input(relative_car_state, lead_reference)
|
||||
|
||||
lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
|
||||
|
||||
print("opt_u = {}".format(lead_opt_u))
|
||||
# input()
|
||||
all_opt_u = np.stack((np.ones(PREDICT_STEP)*V, all_opt_u.flatten()))
|
||||
|
||||
print("opt_u = {}".format(lead_opt_u))
|
||||
print("all_opt_u = {}".format(all_opt_u))
|
||||
|
||||
# predict
|
||||
lead_car.predict_state(all_opt_u, dt=dt)
|
||||
|
||||
# update
|
||||
lead_car.update_state(lead_opt_u, dt=dt)
|
||||
follow_car.update_state(lead_opt_u, dt=dt)
|
||||
|
||||
# print(lead_car.history_predict_xs)
|
||||
# input()
|
||||
|
||||
# figures and animation
|
||||
lead_history_states = np.array(lead_car.history_xs)
|
||||
follow_history_states = np.array(follow_car.history_xs)
|
||||
lead_history_predict_states = lead_car.history_predict_xs
|
||||
|
||||
"""
|
||||
time_history_fig = plt.figure()
|
||||
|
|
@ -394,7 +457,7 @@ def main():
|
|||
plt.show()
|
||||
"""
|
||||
|
||||
animdrawer = AnimDrawer([lead_history_states, follow_history_states, traj_ref])
|
||||
animdrawer = AnimDrawer([lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref])
|
||||
animdrawer.draw_anim()
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
@ -2,7 +2,7 @@ import numpy as np
|
|||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
|
||||
def make_sample_traj(NUM, dt=0.01, a=5.):
|
||||
def make_sample_traj(NUM, dt=0.01, a=30.):
|
||||
"""
|
||||
make sample trajectory
|
||||
Parameters
|
||||
|
|
@ -21,10 +21,9 @@ def make_sample_traj(NUM, dt=0.01, a=5.):
|
|||
traj_ys = []
|
||||
|
||||
for i in range(NUM):
|
||||
traj_xs.append(dt * i)
|
||||
traj_xs.append(i * 0.1)
|
||||
traj_ys.append(a * math.sin(dt * i / DELAY))
|
||||
|
||||
|
||||
plt.plot(traj_xs, traj_ys)
|
||||
plt.show()
|
||||
|
||||
|
|
@ -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