diff --git a/README.md b/README.md index 30216e37..bdb5e830 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,7 @@ Python sample codes and documents about Autonomous vehicle control algorithm. Th * [RRT](#rrt) * [Path Tracking](#path-tracking) * [Pure pursuit Path Tracking](#pure-pursuit-path-tracking) + * [Adaptive Pure pursuit Path Tracking](#adaptive-pure-pursuit-path-tracking) * [Rear wheel feedback Path Tracking](#rear-wheel-feedback-path-tracking) * [LQR(Linear Quadratic Regulator) Path Tracking](#lqrlinear-quadratic-regulator-path-tracking) * [Stanley steering control Path tracking](#stanley-steering-control-path-tracking) @@ -131,6 +132,8 @@ Navigation ### Path Tracking #### Pure pursuit Path Tracking ![](src/simulations/path_tracking/pure_pursuit_path_tracking/pure_pursuit_path_tracking.gif) +#### Adaptive Pure pursuit Path Tracking +![](src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif) #### Rear wheel feedback Path Tracking ![](src/simulations//path_tracking/rear_wheel_feedback_tracking/rear_wheel_feedback_tracking.gif) #### LQR(Linear Quadratic Regulator) Path Tracking diff --git a/src/components/control/pure_pursuit/adaptive_pure_pursuit_controller.py b/src/components/control/pure_pursuit/adaptive_pure_pursuit_controller.py new file mode 100644 index 00000000..c8b3260c --- /dev/null +++ b/src/components/control/pure_pursuit/adaptive_pure_pursuit_controller.py @@ -0,0 +1,142 @@ +""" +adaptive_pure_pursuit_controller.py + +Adaptive Pure Pursuit path tracking controller. Extends pure pursuit by adapting +the look-ahead distance using speed and path curvature: shorter look-ahead on +sharp curves for better tracking, longer look-ahead on straights for stability. +""" + +from math import sin, tan, atan2 + + +class AdaptivePurePursuitController: + """ + Controller class by Adaptive Pure Pursuit algorithm. + Look-ahead distance is adapted by speed (longer at higher speed) and by + path curvature at the current nearest point (shorter on sharp curves). + """ + + def __init__(self, spec, course=None, color='g', + min_look_ahead_m=2.0, + look_forward_gain=0.3, + curvature_adapt_gain=1.0, + max_look_ahead_m=15.0, + speed_proportional_gain=1.0): + """ + Constructor + spec: Vehicle specification object + course: Course data and logic object (with point_curvature optional) + color: Color of drawing target point + min_look_ahead_m: Minimum look-ahead distance [m] + look_forward_gain: Gain for speed-proportional look-ahead + curvature_adapt_gain: Gain for curvature-based reduction (0 = disable) + max_look_ahead_m: Maximum look-ahead distance [m] + speed_proportional_gain: Gain for speed tracking + """ + self.MIN_LOOK_AHEAD_DISTANCE_M = min_look_ahead_m + self.LOOK_FORWARD_GAIN = look_forward_gain + self.CURVATURE_ADAPT_GAIN = curvature_adapt_gain + self.MAX_LOOK_AHEAD_DISTANCE_M = max_look_ahead_m + self.SPEED_PROPORTIONAL_GAIN = speed_proportional_gain + self.WHEEL_BASE_M = spec.wheel_base_m + self.DRAW_COLOR = color + + self.course = course + self.look_ahead_distance_m = self.MIN_LOOK_AHEAD_DISTANCE_M + self.target_course_index = 0 + self.target_accel_mps2 = 0.0 + self.target_speed_mps = 0.0 + self.target_steer_rad = 0.0 + self.target_yaw_rate_rps = 0.0 + + def _calculate_look_ahead_distance(self, state): + """ + Adaptive look-ahead: base term from speed, reduced by path curvature. + L = clamp((L0 + k * v) / (1 + K_curv * |curvature|), L_min, L_max) + """ + base_look_ahead = self.LOOK_FORWARD_GAIN * state.get_speed_mps() + self.MIN_LOOK_AHEAD_DISTANCE_M + + curvature_factor = 1.0 + if self.course and hasattr(self.course, 'point_curvature'): + nearest_index = self.course.search_nearest_point_index(state) + curvature = self.course.point_curvature(nearest_index) + curvature_factor = 1.0 + self.CURVATURE_ADAPT_GAIN * abs(curvature) + + self.look_ahead_distance_m = base_look_ahead / curvature_factor + self.look_ahead_distance_m = max( + self.MIN_LOOK_AHEAD_DISTANCE_M, + min(self.MAX_LOOK_AHEAD_DISTANCE_M, self.look_ahead_distance_m), + ) + + def _calculate_target_course_index(self, state): + """ + Private function to calculate target point's index on course + state: Vehicle's state object + """ + nearest_index = self.course.search_nearest_point_index(state) + while self.look_ahead_distance_m > self.course.calculate_distance_from_point(state, nearest_index): + if nearest_index + 1 >= self.course.length(): + break + nearest_index += 1 + self.target_course_index = nearest_index + + def _decide_target_speed_mps(self): + """Private function to decide target speed [m/s].""" + self.target_speed_mps = self.course.point_speed_mps(self.target_course_index) + + def _calculate_target_acceleration_mps2(self, state): + """Private function to calculate acceleration input.""" + diff_speed_mps = self.target_speed_mps - state.get_speed_mps() + self.target_accel_mps2 = self.SPEED_PROPORTIONAL_GAIN * diff_speed_mps + + def _calculate_target_steer_angle_rad(self, state): + """Private function to calculate steering angle input.""" + diff_angle_rad = self.course.calculate_angle_difference_rad(state, self.target_course_index) + self.target_steer_rad = atan2( + (2 * self.WHEEL_BASE_M * sin(diff_angle_rad)), + self.look_ahead_distance_m, + ) + + def _calculate_target_yaw_rate_rps(self, state): + """Private function to calculate yaw rate input.""" + self.target_yaw_rate_rps = state.get_speed_mps() * tan(self.target_steer_rad) / self.WHEEL_BASE_M + + def update(self, state, time_s): + """ + Function to update data for path tracking + state: Vehicle's state object + time_s: Simulation interval time [sec] + """ + if not self.course: + return + + self._calculate_look_ahead_distance(state) + self._calculate_target_course_index(state) + self._decide_target_speed_mps() + self._calculate_target_acceleration_mps2(state) + self._calculate_target_steer_angle_rad(state) + self._calculate_target_yaw_rate_rps(state) + + def get_target_accel_mps2(self): + """Function to get acceleration input [m/s^2].""" + return self.target_accel_mps2 + + def get_target_steer_rad(self): + """Function to get steering angle input [rad].""" + return self.target_steer_rad + + def get_target_yaw_rate_rps(self): + """Function to get yaw rate input [rad/s].""" + return self.target_yaw_rate_rps + + def draw(self, axes, elems): + """Function to draw target point on course.""" + target_point_plot, = axes.plot( + self.course.point_x_m(self.target_course_index), + self.course.point_y_m(self.target_course_index), + marker='o', + color=self.DRAW_COLOR, + linewidth=0, + label="Target Point", + ) + elems.append(target_point_plot) diff --git a/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif b/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif new file mode 100644 index 00000000..ac807e29 Binary files /dev/null and b/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.gif differ diff --git a/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.py b/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.py new file mode 100644 index 00000000..80aed3a0 --- /dev/null +++ b/src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking/adaptive_pure_pursuit_path_tracking.py @@ -0,0 +1,87 @@ +""" +adaptive_pure_pursuit_path_tracking.py + +Path tracking simulation using Adaptive Pure Pursuit controller. +The look-ahead distance adapts to path curvature: shorter on sharp +turns for tight tracking, longer on straights for stability. +""" + +# import path setting +import sys +from pathlib import Path + +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" + +sys.path.append(abs_dir_path + relative_path + "visualization") +sys.path.append(abs_dir_path + relative_path + "state") +sys.path.append(abs_dir_path + relative_path + "vehicle") +sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course") +sys.path.append(abs_dir_path + relative_path + "control/pure_pursuit") + +# import component modules +from global_xy_visualizer import GlobalXYVisualizer +from min_max import MinMax +from time_parameters import TimeParameters +from vehicle_specification import VehicleSpecification +from state import State +from four_wheels_vehicle import FourWheelsVehicle +from cubic_spline_course import CubicSplineCourse +from adaptive_pure_pursuit_controller import AdaptivePurePursuitController + +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True + + +def main(): + """ + Main process function + """ + # set simulation parameters + x_lim, y_lim = MinMax(-5, 55), MinMax(-20, 25) + gif_path = str( + Path(__file__).absolute().parent + / "adaptive_pure_pursuit_path_tracking.gif" + ) + vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25), + gif_name=gif_path) + + # create course data instance + course = CubicSplineCourse([0.0, 10.0, 25, 40, 50], + [0.0, 4, -12, 20, -13], + 20) + vis.add_object(course) + + # create vehicle's spec instance + spec = VehicleSpecification(area_size=20.0) + + # create vehicle's state instance + state = State(color=spec.color) + + # create adaptive pure pursuit controller instance + adaptive_pure_pursuit = AdaptivePurePursuitController( + spec, + course, + color='g', + min_look_ahead_m=2.0, + look_forward_gain=0.4, + curvature_adapt_gain=2.0, + max_look_ahead_m=25.0, + ) + + # create vehicle instance + vehicle = FourWheelsVehicle(state, spec, controller=adaptive_pure_pursuit) + vis.add_object(vehicle) + + # plot figure is not shown when executed as unit test + if not show_plot: + vis.not_show_plot() + + # show plot figure + vis.draw() + + +# execute main process +if __name__ == "__main__": + main() diff --git a/test/test_adaptive_pure_pursuit_path_tracking.py b/test/test_adaptive_pure_pursuit_path_tracking.py new file mode 100644 index 00000000..cfe2e89e --- /dev/null +++ b/test/test_adaptive_pure_pursuit_path_tracking.py @@ -0,0 +1,19 @@ +""" +Test of path tracking simulation by Adaptive Pure Pursuit algorithm +""" + +from pathlib import Path +import sys +import pytest + +sys.path.append( + str(Path(__file__).absolute().parent) + + "/../src/simulations/path_tracking/adaptive_pure_pursuit_path_tracking" +) +import adaptive_pure_pursuit_path_tracking + + +def test_simulation(): + adaptive_pure_pursuit_path_tracking.show_plot = False + + adaptive_pure_pursuit_path_tracking.main()