Introduction
Path tracking technology is to track a course which is generated with a path planning algorithm. In this entry, I'm introducing a summary and Python sample program of Pure pursuit algorithm which is the most basic and is widely used for Autonomous navigation.
Table of contents
- Introduction
- Table of contents
- Source code at GitHub
- Pure pursuit algorithm
- Implementation of Python program
- Path tracking simulation
Source code at GitHub
All source codes are located at the following GitHub repository.
Pure pursuit controller module
github.com
Path tracking simulation
github.com
Pure pursuit algorithm
Pure pursuit is the most basic path tracking algorithm. It computes the angular velocity command that moves the vehicle from its current position to reach some look-ahead point in front of the vehicle. If you want to know the algorithm in detail, please refer to the following links.
https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf
Look-ahead distance
The look-ahead distance is how far along the course the vehicle should look from the current position to compute the angular velocity command.
This distance can be defined as a parameter and affect the path tracking performance. If the distance was small, it will cause the vehicle to move quickly towards the course. However, the vehicle will overshoot the course and oscillate along it. The distance can be larger to reduce the oscillations, but it will result in larger curvatures near the corners.
Angular velocity computation
Firstly, a target point on the course is decided based on the look-ahead distance. And then, the heading error between the current position and the target point can be computed as follow.
Secondly, the turning radius towards the target point can be computed as follow.
Finally, the angular velocity command to follow the course can be computed based on the turning radius and the forward velocity.
Implementation of Python program
1. Controller module class
A controller module class of Pure pursuit algorithm as follow. This class has the following 5 parameters for computing the angular velocity command.
- Minimum look-ahead distance[m]
- Look forward gain
- Speed proportional gain
- Wheel base of vehicle[m]
- Color for drawing target point
Minimum look-ahead distance is used to limit the distance decreasing. Look forward gain is used for changing the distance based on the current velocity. Speed proportional gain is used for computing the forward acceleration command with simple P control algorithm. Wheel base of vehicle is used for computing the angular velocity command as the parameter of the vehicle. Color for drawing target point is used for drawing the decided target point based on the look-ahead distance on the reference course.
""" pure_pursuit_controller.py Author: Shisato Yano """ from math import sin, tan, atan2 class PurePursuitController: """ Controller class by Pure Pursuit algorithm """ def __init__(self, spec, course=None, color='g'): """ Constructor spec: Vehicle specification object course: Course data and logic object color: Color of drawing target point """ self.MIN_LOOK_AHEAD_DISTANCE_M = 2.0 self.LOOK_FORWARD_GAIN = 0.3 self.SPEED_PROPORTIONAL_GAIN = 1.0 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
2. Computation of Look-ahead distance
A function to compute the look-ahead distance is implemented as follow. The distance can be changed dynamically based on the current forward velocity of the vehicle. This is to make the path tracking more stable.
def _calculate_look_ahead_distance(self, state): """ Private function to calculate look ahead distance to target point state: Vehicle's state object """ self.look_ahead_distance_m = self.LOOK_FORWARD_GAIN * state.get_speed_mps() + self.MIN_LOOK_AHEAD_DISTANCE_M
3. Decision of Target point on Course
A function to decide the target point on the reference course is implemented as follow. The target point is located at the nearest distance from the current position of the vehicle and it is more far than the computed look-ahead distance.
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
4. Computation of Forward acceleration command
The functions to compute the forward acceleration are implemented as follow. The velocity is controlled with simple P control algorithm. The difference of the velocity between the current velocity and target velocity is computed and the acceleration is computed based on the difference towards the target velocity. The target velocity is given from the reference course data.
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 state: Vehicle's state object """ diff_speed_mps = self.target_speed_mps - state.get_speed_mps() self.target_accel_mps2 = self.SPEED_PROPORTIONAL_GAIN * diff_speed_mps
5. Computation of Angular velocity command
Firstly, a function to compute the steering angle command is implemented. The steering angle command can be computed based on the following equation.
def _calculate_target_steer_angle_rad(self, state): """ Private function to calculate steering angle input state: Vehicle's state object """ 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)
Finally, a function to compute the angular velocity command is implemented as follow. The angular velocity command can be computed based on the steering angle command following the kinematic model of the vehicle.
def _calculate_target_yaw_rate_rps(self, state): """ Private function to calculate yaw rate input state: Vehicle's state object """ self.target_yaw_rate_rps = state.get_speed_mps() * tan(self.target_steer_rad) / self.WHEEL_BASE_M
6. Sequence of computation
The above implemented functions are executed as follow. The current state of the vehicle is given to the functions. If any reference course data was not given to the controller module, the process of computation is can not be executed.
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)
Path tracking simulation
The path tracking simulation with the above controller module can be executed as follow. The green point on the course is the decided target point based on the current forward velocity. And then, the angular velocity command towards the target point and the vehicle can track the course based on the command. However, the lateral error of the vehicle towards the course still looks like be large around the corners because Pure pursuit algorithm assume that the vehicle approaches the target point following turning behavior.
