github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 21:28:33 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #299 from AtsushiSakai/issue_298

Browse Source 
To fix pure pursuit bug #298
This commit is contained in:
Atsushi Sakai
2020-02-26 22:39:42 +09:00
committed by GitHub
parent e8ffc01cc7 2e188f866b
commit ef39fc2e09
1 changed files with 43 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -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 numpy as np
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 @@ class State:
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 @@ class States:
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 @@ class States:
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 @@ class Trajectory:
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")