github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 09:08:01 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #303 from Chachay/MPC

Browse Source 
[Proposal] Reduce internal dependancy
This commit is contained in:
Atsushi Sakai
2020-03-26 20:38:44 +09:00
committed by GitHub
parent 55478fa01d b6e89c6d8b
commit 9274aacefb
1 changed files with 95 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
View File

@@ -8,14 +8,9 @@ author: Atsushi Sakai(@Atsushi_twi)
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
@@ -26,34 +21,80 @@ dt = 0.1 # [s]
L = 2.9 # [m]
show_animation = True
# show_animation = False
class State:
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direction=1):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
self.direction = direction
def update(self, a, delta, dt):
self.x = self.x + self.v * math.cos(self.yaw) * dt
self.y = self.y + self.v * math.sin(self.yaw) * dt
self.yaw = self.yaw + self.v / L * math.tan(delta) * dt
self.v = self.v + a * dt
def update(state, a, delta):
class CubicSplinePath:
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)
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 / L * math.tan(delta) * dt
state.v = state.v + a * dt
self.X = interpolate.CubicSpline(s, x)
self.Y = interpolate.CubicSpline(s, y)
return state
self.dX = self.X.derivative(1)
self.ddX = self.X.derivative(2)
self.dY = self.Y.derivative(1)
self.ddY = self.Y.derivative(2)
def PIDControl(target, current):
self.length = s[-1]
def calc_yaw(self, s):
dx, dy = self.dX(s), self.dY(s)
return np.arctan2(dy, dx)
def calc_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 __find_nearest_point(self, s0, x, y):
def calc_distance(_s, *args):
_x, _y= self.X(_s), self.Y(_s)
return (_x - args[0])**2 + (_y - args[1])**2
def calc_distance_jacobian(_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(calc_distance, s0, calc_distance_jacobian, args=(x, y), full_output=True, disp=False)
return minimum
def calc_track_error(self, x, y, s0):
ret = self.__find_nearest_point(s0, x, y)
s = ret[0][0]
e = ret[1]
k = self.calc_curvature(s)
yaw = self.calc_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 pid_control(target, current):
a = Kp * (target - current)
return a
def pi_2_pi(angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
@@ -63,53 +104,24 @@ 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_ref):
v = state.v
th_e = pi_2_pi(state.yaw - cyaw[ind])
th_e = pi_2_pi(state.yaw - yaw_ref)
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)
# print(k, v, e, th_e, omega, delta)
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 simulate(path_ref, goal):
T = 500.0 # max simulation time
goal_dis = 0.3
stop_speed = 0.05
state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0)
@@ -120,16 +132,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, path_ref.length, 0.1)
e, k, yaw_ref, s0 = path_ref.calc_track_error(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)
state = update(state, ai, di)
e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, s0)
di = rear_wheel_feedback_control(state, e, k, yaw_ref)
if abs(state.v) <= stop_speed:
target_ind += 1
speed_ref = calc_target_speed(state, yaw_ref)
ai = pid_control(speed_ref, state.v)
state.update(ai, di, dt)
time = time + dt
@@ -147,49 +160,35 @@ 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(path_ref.X(s), path_ref.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(path_ref.X(s0), path_ref.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_target_speed(state, yaw_ref):
target_speed = 10.0 / 3.6
def calc_speed_profile(cx, cy, cyaw, target_speed):
dyaw = yaw_ref - state.yaw
switch = math.pi / 4.0 <= dyaw < math.pi / 2.0
speed_profile = [target_speed] * len(cx)
direction = 1.0
# Set stop point
for i in range(len(cx) - 1):
dyaw = cyaw[i + 1] - cyaw[i]
switch = math.pi / 4.0 <= dyaw < math.pi / 2.0
if switch:
direction *= -1
if direction != 1.0:
speed_profile[i] = - target_speed
else:
speed_profile[i] = target_speed
if switch:
speed_profile[i] = 0.0
speed_profile[-1] = 0.0
return speed_profile
if switch:
state.direction *= -1
return 0.0
if state.direction != 1:
return -target_speed
return target_speed
def main():
print("rear wheel feedback tracking start!!")
@@ -197,14 +196,10 @@ 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)
target_speed = 10.0 / 3.6
reference_path = CubicSplinePath(ax, ay)
s = np.arange(0, reference_path.length, 0.1)
sp = calc_speed_profile(cx, cy, cyaw, 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 = simulate(reference_path, goal)
# Test
assert goal_flag, "Cannot goal"
@@ -213,7 +208,7 @@ def main():
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(reference_path.X(s), reference_path.Y(s), "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
@@ -222,14 +217,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(reference_path.calc_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, reference_path.calc_curvature(s), "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
@@ -237,6 +232,5 @@ def main():
plt.show()
if __name__ == '__main__':
main()