From 2e188f866b0de3e46cca5e3b9f807003be8bc305 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Wed, 26 Feb 2020 22:27:25 +0900 Subject: [PATCH] To fix #298 --- PathTracking/pure_pursuit/pure_pursuit.py | 83 ++++++++++++----------- 1 file changed, 43 insertions(+), 40 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index bcaf8697265..ff995033a9e 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -1,6 +1,6 @@ """ -Path tracking simulation with pure pursuit steering control and PID speed control. +Path tracking simulation with pure pursuit steering and PID speed control. author: Atsushi Sakai (@Atsushi_twi) Guillaume Jacquenot (@Gjacquenot) @@ -10,13 +10,12 @@ import math import matplotlib.pyplot as plt - +# Parameters k = 0.1 # look forward gain -Lfc = 2.0 # look-ahead distance +Lfc = 2.0 # [m] look-ahead distance Kp = 1.0 # speed proportional gain -dt = 0.1 # [s] -L = 2.9 # [m] wheel base of vehicle - +dt = 0.1 # [s] time tick +WB = 2.9 # [m] wheel base of vehicle show_animation = True @@ -28,20 +27,18 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): self.y = y self.yaw = yaw self.v = v - self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) - self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) + self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw)) + self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw)) def update(self, a, delta): - self.x += self.v * math.cos(self.yaw) * dt self.y += self.v * math.sin(self.yaw) * dt - self.yaw += self.v / L * math.tan(delta) * dt + self.yaw += self.v / WB * math.tan(delta) * dt self.v += a * dt - self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) - self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) + self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw)) + self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw)) def calc_distance(self, point_x, point_y): - dx = self.rear_x - point_x dy = self.rear_y - point_y return math.hypot(dx, dy) @@ -56,7 +53,7 @@ def __init__(self): self.v = [] self.t = [] - def append(self, t , state): + def append(self, t, state): self.x.append(state.x) self.y.append(state.y) self.yaw.append(state.yaw) @@ -64,19 +61,22 @@ def append(self, t , state): self.t.append(t) -def PIDControl(target, current): +def proportional_control(target, current): a = Kp * (target - current) return a -class Trajectory: +class TargetCourse: + def __init__(self, cx, cy): self.cx = cx self.cy = cy self.old_nearest_point_index = None def search_target_index(self, state): + + # To speed up nearest point search, doing it at only first time. if self.old_nearest_point_index is None: # search nearest point index dx = [state.rear_x - icx for icx in self.cx] @@ -86,30 +86,30 @@ def search_target_index(self, state): self.old_nearest_point_index = ind else: ind = self.old_nearest_point_index - distance_this_index = state.calc_distance(self.cx[ind], self.cy[ind]) + distance_this_index = state.calc_distance(self.cx[ind], + self.cy[ind]) while True: - ind = ind + 1 if (ind + 1) < len(self.cx) else ind - distance_next_index = state.calc_distance(self.cx[ind], self.cy[ind]) + distance_next_index = state.calc_distance(self.cx[ind + 1], + self.cy[ind + 1]) if distance_this_index < distance_next_index: break + ind = ind + 1 if (ind + 1) < len(self.cx) else ind distance_this_index = distance_next_index self.old_nearest_point_index = ind - L = 0.0 - - Lf = k * state.v + Lfc + Lf = k * state.v + Lfc # update look ahead distance # search look ahead target point index - while Lf > L and (ind + 1) < len(self.cx): - L = state.calc_distance(self.cx[ind], self.cy[ind]) + while Lf > state.calc_distance(self.cx[ind], self.cy[ind]): + if (ind + 1) >= len(self.cx): + break # not exceed goal ind += 1 - return ind + return ind, Lf -def pure_pursuit_control(state, trajectory, pind): - - ind = trajectory.search_target_index(state) +def pure_pursuit_steer_control(state, trajectory, pind): + ind, Lf = trajectory.search_target_index(state) if pind >= ind: ind = pind @@ -117,16 +117,14 @@ def pure_pursuit_control(state, trajectory, pind): if ind < len(trajectory.cx): tx = trajectory.cx[ind] ty = trajectory.cy[ind] - else: + else: # toward goal tx = trajectory.cx[-1] ty = trajectory.cy[-1] ind = len(trajectory.cx) - 1 alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw - Lf = k * state.v + Lfc - - delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) + delta = math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0) return delta, ind @@ -147,7 +145,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def main(): # target course - cx = np.arange(0, 50, 0.1) + cx = np.arange(0, 50, 0.5) cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx] target_speed = 10.0 / 3.6 # [m/s] @@ -161,13 +159,17 @@ def main(): time = 0.0 states = States() states.append(time, state) - trajectory = Trajectory(cx, cy) - target_ind = trajectory.search_target_index(state) + target_course = TargetCourse(cx, cy) + target_ind, _ = target_course.search_target_index(state) while T >= time and lastIndex > target_ind: - ai = PIDControl(target_speed, state.v) - di, target_ind = pure_pursuit_control(state, trajectory, target_ind) - state.update(ai, di) + + # Calc control input + ai = proportional_control(target_speed, state.v) + di, target_ind = pure_pursuit_steer_control( + state, target_course, target_ind) + + state.update(ai, di) # Control vehicle time += dt states.append(time, state) @@ -175,8 +177,9 @@ def main(): if show_animation: # pragma: no cover 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.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plot_arrow(state.x, state.y, state.yaw) plt.plot(cx, cy, "-r", label="course") plt.plot(states.x, states.y, "-b", label="trajectory")