Skip to content

Commit e08db27

Browse files
ShisatoYanoShisatoYano
authored
Merge pull request #46 from guilyx/feature/adaptive-pure-pursuit
Control: Adaptive Pure Pursuit
2 parents 97c25e0 + 00317b2 commit e08db27

5 files changed

Lines changed: 251 additions & 0 deletions

File tree

‎README.md‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ Python sample codes and documents about Autonomous vehicle control algorithm. Th
3131
* [Informed RRT*](#informed-rrt)
3232
* [Path Tracking](#path-tracking)
3333
* [Pure pursuit Path Tracking](#pure-pursuit-path-tracking)
34+
* [Adaptive Pure pursuit Path Tracking](#adaptive-pure-pursuit-path-tracking)
3435
* [Rear wheel feedback Path Tracking](#rear-wheel-feedback-path-tracking)
3536
* [LQR(Linear Quadratic Regulator) Path Tracking](#lqrlinear-quadratic-regulator-path-tracking)
3637
* [Stanley steering control Path tracking](#stanley-steering-control-path-tracking)
@@ -143,6 +144,8 @@ Navigation
143144
### Path Tracking
144145
#### Pure pursuit Path Tracking
145146
![](src/simulations/path_tracking/pure_pursuit_path_tracking/pure_pursuit_path_tracking.gif)
147+
#### Adaptive Pure pursuit Path Tracking
148+
![](src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif)
146149
#### Rear wheel feedback Path Tracking
147150
![](src/simulations//path_tracking/rear_wheel_feedback_tracking/rear_wheel_feedback_tracking.gif)
148151
#### LQR(Linear Quadratic Regulator) Path Tracking

‎src/components/control/pure_pursuit/adaptive_pure_pursuit_controller.py‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
"""
2+
adaptive_pure_pursuit_controller.py
3+
4+
Adaptive Pure Pursuit path tracking controller. Extends pure pursuit by adapting
5+
the look-ahead distance using speed and path curvature: shorter look-ahead on
6+
sharp curves for better tracking, longer look-ahead on straights for stability.
7+
"""
8+
9+
from math import sin, tan, atan2
10+
11+
12+
class AdaptivePurePursuitController:
13+
"""
14+
Controller class by Adaptive Pure Pursuit algorithm.
15+
Look-ahead distance is adapted by speed (longer at higher speed) and by
16+
path curvature at the current nearest point (shorter on sharp curves).
17+
"""
18+
19+
def __init__(self, spec, course=None, color='g',
20+
min_look_ahead_m=2.0,
21+
look_forward_gain=0.3,
22+
curvature_adapt_gain=1.0,
23+
max_look_ahead_m=15.0,
24+
speed_proportional_gain=1.0):
25+
"""
26+
Constructor
27+
spec: Vehicle specification object
28+
course: Course data and logic object (with point_curvature optional)
29+
color: Color of drawing target point
30+
min_look_ahead_m: Minimum look-ahead distance [m]
31+
look_forward_gain: Gain for speed-proportional look-ahead
32+
curvature_adapt_gain: Gain for curvature-based reduction (0 = disable)
33+
max_look_ahead_m: Maximum look-ahead distance [m]
34+
speed_proportional_gain: Gain for speed tracking
35+
"""
36+
self.MIN_LOOK_AHEAD_DISTANCE_M = min_look_ahead_m
37+
self.LOOK_FORWARD_GAIN = look_forward_gain
38+
self.CURVATURE_ADAPT_GAIN = curvature_adapt_gain
39+
self.MAX_LOOK_AHEAD_DISTANCE_M = max_look_ahead_m
40+
self.SPEED_PROPORTIONAL_GAIN = speed_proportional_gain
41+
self.WHEEL_BASE_M = spec.wheel_base_m
42+
self.DRAW_COLOR = color
43+
44+
self.course = course
45+
self.look_ahead_distance_m = self.MIN_LOOK_AHEAD_DISTANCE_M
46+
self.target_course_index = 0
47+
self.target_accel_mps2 = 0.0
48+
self.target_speed_mps = 0.0
49+
self.target_steer_rad = 0.0
50+
self.target_yaw_rate_rps = 0.0
51+
52+
def _calculate_look_ahead_distance(self, state):
53+
"""
54+
Adaptive look-ahead: base term from speed, reduced by path curvature.
55+
L = clamp((L0 + k * v) / (1 + K_curv * |curvature|), L_min, L_max)
56+
"""
57+
base_look_ahead = self.LOOK_FORWARD_GAIN * state.get_speed_mps() + self.MIN_LOOK_AHEAD_DISTANCE_M
58+
59+
curvature_factor = 1.0
60+
if self.course and hasattr(self.course, 'point_curvature'):
61+
nearest_index = self.course.search_nearest_point_index(state)
62+
curvature = self.course.point_curvature(nearest_index)
63+
curvature_factor = 1.0 + self.CURVATURE_ADAPT_GAIN * abs(curvature)
64+
65+
self.look_ahead_distance_m = base_look_ahead / curvature_factor
66+
self.look_ahead_distance_m = max(
67+
self.MIN_LOOK_AHEAD_DISTANCE_M,
68+
min(self.MAX_LOOK_AHEAD_DISTANCE_M, self.look_ahead_distance_m),
69+
)
70+
71+
def _calculate_target_course_index(self, state):
72+
"""
73+
Private function to calculate target point's index on course
74+
state: Vehicle's state object
75+
"""
76+
nearest_index = self.course.search_nearest_point_index(state)
77+
while self.look_ahead_distance_m > self.course.calculate_distance_from_point(state, nearest_index):
78+
if nearest_index + 1 >= self.course.length():
79+
break
80+
nearest_index += 1
81+
self.target_course_index = nearest_index
82+
83+
def _decide_target_speed_mps(self):
84+
"""Private function to decide target speed [m/s]."""
85+
self.target_speed_mps = self.course.point_speed_mps(self.target_course_index)
86+
87+
def _calculate_target_acceleration_mps2(self, state):
88+
"""Private function to calculate acceleration input."""
89+
diff_speed_mps = self.target_speed_mps - state.get_speed_mps()
90+
self.target_accel_mps2 = self.SPEED_PROPORTIONAL_GAIN * diff_speed_mps
91+
92+
def _calculate_target_steer_angle_rad(self, state):
93+
"""Private function to calculate steering angle input."""
94+
diff_angle_rad = self.course.calculate_angle_difference_rad(state, self.target_course_index)
95+
self.target_steer_rad = atan2(
96+
(2 * self.WHEEL_BASE_M * sin(diff_angle_rad)),
97+
self.look_ahead_distance_m,
98+
)
99+
100+
def _calculate_target_yaw_rate_rps(self, state):
101+
"""Private function to calculate yaw rate input."""
102+
self.target_yaw_rate_rps = state.get_speed_mps() * tan(self.target_steer_rad) / self.WHEEL_BASE_M
103+
104+
def update(self, state, time_s):
105+
"""
106+
Function to update data for path tracking
107+
state: Vehicle's state object
108+
time_s: Simulation interval time [sec]
109+
"""
110+
if not self.course:
111+
return
112+
113+
self._calculate_look_ahead_distance(state)
114+
self._calculate_target_course_index(state)
115+
self._decide_target_speed_mps()
116+
self._calculate_target_acceleration_mps2(state)
117+
self._calculate_target_steer_angle_rad(state)
118+
self._calculate_target_yaw_rate_rps(state)
119+
120+
def get_target_accel_mps2(self):
121+
"""Function to get acceleration input [m/s^2]."""
122+
return self.target_accel_mps2
123+
124+
def get_target_steer_rad(self):
125+
"""Function to get steering angle input [rad]."""
126+
return self.target_steer_rad
127+
128+
def get_target_yaw_rate_rps(self):
129+
"""Function to get yaw rate input [rad/s]."""
130+
return self.target_yaw_rate_rps
131+
132+
def draw(self, axes, elems):
133+
"""Function to draw target point on course."""
134+
target_point_plot, = axes.plot(
135+
self.course.point_x_m(self.target_course_index),
136+
self.course.point_y_m(self.target_course_index),
137+
marker='o',
138+
color=self.DRAW_COLOR,
139+
linewidth=0,
140+
label="Target Point",
141+
)
142+
elems.append(target_point_plot)

‎src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif‎

2.65 MB
Loading

‎src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.py‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
"""
2+
adaptive_pure_pursuit_path_tracking.py
3+
4+
Path tracking simulation using Adaptive Pure Pursuit controller.
5+
The look-ahead distance adapts to path curvature: shorter on sharp
6+
turns for tight tracking, longer on straights for stability.
7+
"""
8+
9+
# import path setting
10+
import sys
11+
from pathlib import Path
12+
13+
abs_dir_path = str(Path(__file__).absolute().parent)
14+
relative_path = "/../../../components/"
15+
16+
sys.path.append(abs_dir_path + relative_path + "visualization")
17+
sys.path.append(abs_dir_path + relative_path + "state")
18+
sys.path.append(abs_dir_path + relative_path + "vehicle")
19+
sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")
20+
sys.path.append(abs_dir_path + relative_path + "control/pure_pursuit")
21+
22+
# import component modules
23+
from global_xy_visualizer import GlobalXYVisualizer
24+
from min_max import MinMax
25+
from time_parameters import TimeParameters
26+
from vehicle_specification import VehicleSpecification
27+
from state import State
28+
from four_wheels_vehicle import FourWheelsVehicle
29+
from cubic_spline_course import CubicSplineCourse
30+
from adaptive_pure_pursuit_controller import AdaptivePurePursuitController
31+
32+
# flag to show plot figure
33+
# when executed as unit test, this flag is set as false
34+
show_plot = True
35+
36+
37+
def main():
38+
"""
39+
Main process function
40+
"""
41+
# set simulation parameters
42+
x_lim, y_lim = MinMax(-5, 55), MinMax(-20, 25)
43+
gif_path = str(
44+
Path(__file__).absolute().parent
45+
/ "adaptive_pure_pursuit_path_tracking.gif"
46+
)
47+
vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25),
48+
gif_name=gif_path)
49+
50+
# create course data instance
51+
course = CubicSplineCourse([0.0, 10.0, 25, 40, 50],
52+
[0.0, 4, -12, 20, -13],
53+
20)
54+
vis.add_object(course)
55+
56+
# create vehicle's spec instance
57+
spec = VehicleSpecification(area_size=20.0)
58+
59+
# create vehicle's state instance
60+
state = State(color=spec.color)
61+
62+
# create adaptive pure pursuit controller instance
63+
adaptive_pure_pursuit = AdaptivePurePursuitController(
64+
spec,
65+
course,
66+
color='g',
67+
min_look_ahead_m=2.0,
68+
look_forward_gain=0.4,
69+
curvature_adapt_gain=2.0,
70+
max_look_ahead_m=25.0,
71+
)
72+
73+
# create vehicle instance
74+
vehicle = FourWheelsVehicle(state, spec, controller=adaptive_pure_pursuit)
75+
vis.add_object(vehicle)
76+
77+
# plot figure is not shown when executed as unit test
78+
if not show_plot:
79+
vis.not_show_plot()
80+
81+
# show plot figure
82+
vis.draw()
83+
84+
85+
# execute main process
86+
if __name__ == "__main__":
87+
main()

‎test/test_adaptive_pure_pursuit_path_tracking.py‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
"""
2+
Test of path tracking simulation by Adaptive Pure Pursuit algorithm
3+
"""
4+
5+
from pathlib import Path
6+
import sys
7+
import pytest
8+
9+
sys.path.append(
10+
str(Path(__file__).absolute().parent)
11+
+ "/../src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking"
12+
)
13+
import adaptive_pure_pursuit_path_tracking
14+
15+
16+
def test_simulation():
17+
adaptive_pure_pursuit_path_tracking.show_plot = False
18+
19+
adaptive_pure_pursuit_path_tracking.main()

0 commit comments

Comments
 (0)