mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
Replace Spline module to Scipy interpolate
This commit is contained in:
Chachay
@@ -9,12 +9,9 @@ import matplotlib.pyplot as plt
|
||||
import math
|
||||
import numpy as np
|
||||
import sys
|
||||
sys.path.append("../../PathPlanning/CubicSpline/")
|
||||
|
||||
try:
|
||||
import cubic_spline_planner
|
||||
except:
|
||||
raise
|
||||
from scipy import interpolate
|
||||
from scipy import optimize
|
||||
|
||||
Kp = 1.0 # speed propotional gain
|
||||
# steering control parameter
|
||||
@@ -39,6 +36,61 @@ class State:
|
||||
self.yaw = self.yaw + self.v / L * math.tan(delta) * dt
|
||||
self.v = self.v + a * dt
|
||||
|
||||
class TrackSpline:
|
||||
def __init__(self, x, y):
|
||||
x, y = map(np.asarray, (x, y))
|
||||
s = np.append([0],(np.cumsum(np.diff(x)**2) + np.cumsum(np.diff(y)**2))**0.5)
|
||||
|
||||
self.X = interpolate.CubicSpline(s, x)
|
||||
self.Y = interpolate.CubicSpline(s, y)
|
||||
|
||||
self.dX = self.X.derivative(1)
|
||||
self.ddX = self.X.derivative(2)
|
||||
|
||||
self.dY = self.Y.derivative(1)
|
||||
self.ddY = self.Y.derivative(2)
|
||||
|
||||
self.length = s[-1]
|
||||
|
||||
def yaw(self, s):
|
||||
dx, dy = self.dX(s), self.dY(s)
|
||||
return np.arctan2(dy, dx)
|
||||
|
||||
def curvature(self, s):
|
||||
dx, dy = self.dX(s), self.dY(s)
|
||||
ddx, ddy = self.ddX(s), self.ddY(s)
|
||||
return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
|
||||
|
||||
def __findClosestPoint(self, s0, x, y):
|
||||
def f(_s, *args):
|
||||
_x, _y= self.X(_s), self.Y(_s)
|
||||
return (_x - args[0])**2 + (_y - args[1])**2
|
||||
|
||||
def jac(_s, *args):
|
||||
_x, _y = self.X(_s), self.Y(_s)
|
||||
_dx, _dy = self.dX(_s), self.dY(_s)
|
||||
return 2*_dx*(_x - args[0])+2*_dy*(_y-args[1])
|
||||
|
||||
minimum = optimize.fmin_cg(f, s0, jac, args=(x, y), full_output=True, disp=False)
|
||||
return minimum
|
||||
|
||||
def TrackError(self, x, y, s0):
|
||||
ret = self.__findClosestPoint(s0, x, y)
|
||||
|
||||
s = ret[0][0]
|
||||
e = ret[1]
|
||||
|
||||
k = self.curvature(s)
|
||||
yaw = self.yaw(s)
|
||||
|
||||
dxl = self.X(s) - x
|
||||
dyl = self.Y(s) - y
|
||||
angle = pi_2_pi(yaw - math.atan2(dyl, dxl))
|
||||
if angle < 0:
|
||||
e*= -1
|
||||
|
||||
return e, k, yaw, s
|
||||
|
||||
def PIDControl(target, current):
|
||||
a = Kp * (target - current)
|
||||
return a
|
||||
@@ -52,46 +104,22 @@ def pi_2_pi(angle):
|
||||
|
||||
return angle
|
||||
|
||||
def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind):
|
||||
ind, e = calc_nearest_index(state, cx, cy, cyaw)
|
||||
|
||||
k = ck[ind]
|
||||
def rear_wheel_feedback_control(state, e, k, yaw_r):
|
||||
v = state.v
|
||||
th_e = pi_2_pi(state.yaw - cyaw[ind])
|
||||
th_e = pi_2_pi(state.yaw - yaw_r)
|
||||
|
||||
omega = v * k * math.cos(th_e) / (1.0 - k * e) - \
|
||||
KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e
|
||||
|
||||
if th_e == 0.0 or omega == 0.0:
|
||||
return 0.0, ind
|
||||
return 0.0
|
||||
|
||||
delta = math.atan2(L * omega / v, 1.0)
|
||||
|
||||
return delta, ind
|
||||
return delta
|
||||
|
||||
|
||||
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 = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
|
||||
|
||||
mind = min(d)
|
||||
|
||||
ind = d.index(mind)
|
||||
|
||||
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 closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
def closed_loop_prediction(track, speed_profile, goal):
|
||||
T = 500.0 # max simulation time
|
||||
goal_dis = 0.3
|
||||
stop_speed = 0.05
|
||||
@@ -105,17 +133,17 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
v = [state.v]
|
||||
t = [0.0]
|
||||
goal_flag = False
|
||||
target_ind = calc_nearest_index(state, cx, cy, cyaw)
|
||||
|
||||
s = np.arange(0, track.length, 0.1)
|
||||
e, k, yaw_r, s0 = track.TrackError(state.x, state.y, 0.0)
|
||||
|
||||
while T >= time:
|
||||
di, target_ind = rear_wheel_feedback_control(
|
||||
state, cx, cy, cyaw, ck, target_ind)
|
||||
ai = PIDControl(speed_profile[target_ind], state.v)
|
||||
e, k, yaw_r, s0 = track.TrackError(state.x, state.y, s0)
|
||||
di = rear_wheel_feedback_control(state, e, k, yaw_r)
|
||||
#ai = PIDControl(speed_profile[target_ind], state.v)
|
||||
ai = PIDControl(speed_profile, state.v)
|
||||
state.update(ai, di, dt)
|
||||
|
||||
if abs(state.v) <= stop_speed:
|
||||
target_ind += 1
|
||||
|
||||
time = time + dt
|
||||
|
||||
# check goal
|
||||
@@ -132,23 +160,22 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
|
||||
v.append(state.v)
|
||||
t.append(time)
|
||||
|
||||
if target_ind % 1 == 0 and show_animation:
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.plot(cx, cy, "-r", label="course")
|
||||
plt.plot(track.X(s), track.Y(s), "-r", label="course")
|
||||
plt.plot(x, y, "ob", label="trajectory")
|
||||
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
|
||||
plt.plot(track.X(s0), track.Y(s0), "xg", label="target")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) +
|
||||
",target index:" + str(target_ind))
|
||||
plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0))
|
||||
plt.pause(0.0001)
|
||||
|
||||
return t, x, y, yaw, v, goal_flag
|
||||
|
||||
def calc_speed_profile(cx, cy, cyaw, target_speed):
|
||||
def calc_speed_profile(track, target_speed, s):
|
||||
speed_profile = [target_speed] * len(cx)
|
||||
direction = 1.0
|
||||
|
||||
@@ -178,14 +205,16 @@ def main():
|
||||
ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
|
||||
goal = [ax[-1], ay[-1]]
|
||||
|
||||
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
|
||||
ax, ay, ds=0.1)
|
||||
track = TrackSpline(ax, ay)
|
||||
s = np.arange(0, track.length, 0.1)
|
||||
|
||||
target_speed = 10.0 / 3.6
|
||||
|
||||
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
|
||||
# Note: disable backward direction temporary
|
||||
#sp = calc_speed_profile(track, target_speed, s)
|
||||
sp = target_speed
|
||||
|
||||
t, x, y, yaw, v, goal_flag = closed_loop_prediction(
|
||||
cx, cy, cyaw, ck, sp, goal)
|
||||
t, x, y, yaw, v, goal_flag = closed_loop_prediction(track, sp, goal)
|
||||
|
||||
# Test
|
||||
assert goal_flag, "Cannot goal"
|
||||
@@ -194,7 +223,7 @@ def main():
|
||||
plt.close()
|
||||
plt.subplots(1)
|
||||
plt.plot(ax, ay, "xb", label="input")
|
||||
plt.plot(cx, cy, "-r", label="spline")
|
||||
plt.plot(track.X(s), track.Y(s), "-r", label="spline")
|
||||
plt.plot(x, y, "-g", label="tracking")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
@@ -203,14 +232,14 @@ def main():
|
||||
plt.legend()
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
|
||||
plt.plot(s, np.rad2deg(track.yaw(s)), "-r", label="yaw")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.xlabel("line length[m]")
|
||||
plt.ylabel("yaw angle[deg]")
|
||||
|
||||
plt.subplots(1)
|
||||
plt.plot(s, ck, "-r", label="curvature")
|
||||
plt.plot(s, track.curvature(s), "-r", label="curvature")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.xlabel("line length[m]")
|
||||
|
||||
Reference in New Issue
Block a user