In the previous articles, we discussed the various components of autonomous vehicle technology, from perception systems to advanced control strategies. Now, we transition from theoretical concepts to practical implementation, focusing on the mathematical underpinnings and coding aspects of longitudinal and lateral controls using PID controllers. This article aims to demystify the technical processes that enable precise vehicle handling and maneuvering.
Let’s delve deeper into the different types of control strategies used in the longitudinal and lateral control systems of autonomous vehicles. We’ll explore feedforward, feedback, and their combination for longitudinal control and compare Pure Pursuit, Stanley, and Model Predictive Control (MPC) methods for lateral control. Additionally, we’ll highlight the mathematical principles behind these strategies and discuss the advantages and challenges associated with each.
Feedforward Control
Feedforward control uses predictive models to set control actions based on expected conditions rather than relying solely on feedback from current states. This preemptive approach can lead to more responsive and stable system behavior.
Mathematical Principle: Feedforward control calculations typically involve using known system dynamics and environmental inputs (like road slope or expected acceleration) to predict the required control actions.
Feedback Control
Feedback control adjusts the vehicle’s actions based on the error between the desired state and the actual state. This is useful for correcting unforeseen disturbances and errors that the system encounters.
Mathematical Principle: The feedback control often uses PID controllers, which calculate the error between the desired and actual states and apply corrections based on proportional, integral, and derivative terms.
Combination Control
Definition: Combining feedforward and feedback control leverages the strengths of both approaches: the anticipatory nature of feedforward control with the error-correcting capabilities of feedback control.
# Parameters and states (assumed defined elsewhere):
# v_desired: desired velocity
# v: current velocity
# t: current time
# kp, ki, kd: PID gains
# road_slope, get_road_slope(), calculate_expected_acceleration(): defined functions
# self.vars: object storing persistent variables (e.g., e_previous, E, t_last)
# throttle_output, brake_output: control outputs in [0, 1]# Feedforward Calculation
road_slope = get_road_slope()
expected_acceleration = calculate_expected_acceleration(road_slope)
feedforward_throttle = calculate_throttle_from_acceleration(expected_acceleration)
# Feedback (PID) Calculation
delta_t = t - self.vars.t_last
e_current = v_desired - v # Speed error
Proportional = kp * e_current
self.vars.E += e_current * delta_t
Integral = ki * self.vars.E
Derivative = kd * (e_current - self.vars.e_previous) / delta_t if delta_t > 0 else 0
pid_control = Proportional + Integral + Derivative
# Combine Feedforward and Feedback
total_control_input = feedforward_throttle + pid_control
# Determine throttle and brake commands
if total_control_input >= 0:
throttle_output = min(max(total_control_input, 0), 1)
brake_output = 0
else:
throttle_output = 0
brake_output = min(max(-total_control_input, 0), 1)
# Update memory variables
self.vars.e_previous = e_current
self.vars.t_last = t
Pure Pursuit
Pure Pursuit is a path-tracking algorithm that calculates the required steering angle to follow a path based on a look-ahead point, which is a point on the path at a certain distance ahead of the vehicle.
Disadvantages: Pure Pursuit may not perform well at high speeds or in tight curves because it doesn’t account for vehicle dynamics.
Stanley Method
The Stanley method combines the cross-track error (distance from the vehicle to the path) and the heading error (difference in orientation from the path) to compute the steering command.
In addition to the cross-track error, autonomous vehicles must also calculate the heading error to ensure the vehicle’s orientation aligns with the trajectory of the road. This error is the angular difference between the vehicle’s current heading and the direction of the trajectory defined by the waypoints. Here’s how it’s computed:
# Calculate heading error
desired_heading = np.arctan2(waypoints[-1][1] - waypoints[0][1], waypoints[-1][0] - waypoints[0][0])
current_heading = yaw_path # Assume yaw_path represents the vehicle's current heading
heading_error = desired_heading - current_heading# Normalize the heading error to be within the range of -pi to pi
if heading_error > np.pi:
heading_error -= 2 * np.pi
elif heading_error < -np.pi:
heading_error += 2 * np.pi
When an autonomous vehicle follows a path, it is essential to continually adjust its trajectory to align with a set of predefined waypoints. These waypoints outline the intended route. The following pseudocode shows how we calculate the cross-track error, which measures the lateral distance between the vehicle’s current position and the nearest point on the desired path:
# Define waypoints and vehicle current position
waypoints = [(1, 1), (5, 5)]
x, y = 2.5, 2 # Current vehicle position
yaw_path = np.pi / 4 # Current path yaw# Calculate the slope from the waypoints
slope = (waypoints[-1][1] - waypoints[0][1]) / (waypoints[-1][0] - waypoints[0][0])
a = -slope
b = 1.0
c = (slope * waypoints[0][0]) - waypoints[0][1]
# Calculate the cross-track error
cross_track_error = (a * x + b * y + c) / np.sqrt(a**2 + b**2)
# Adjust error based on vehicle orientation
yaw_cross_track = np.arctan2(y - waypoints[0][1], x - waypoints[0][0])
yaw_path_to_ct = yaw_path - yaw_cross_track
if yaw_path_to_ct > np.pi:
yaw_path_to_ct -= 2 * np.pi
elif yaw_path_to_ct < -np.pi:
yaw_path_to_ct += 2 * np.pi
cross_track_error = abs(cross_track_error) if yaw_path_to_ct > 0 else -abs(cross_track_error)
To implement this combined steering control, the vehicle’s control system needs to continuously calculate both types of errors and adjust the steering angle accordingly:
def compute_steering_correction(waypoints, current_position, current_heading, velocity):
# Calculate slope of the path from waypoints
slope = (waypoints[-1][1] - waypoints[0][1]) / (waypoints[-1][0] - waypoints[0][0])
a = -slope
b = 1.0
c = (slope * waypoints[0][0]) - waypoints[0][1]# Calculate cross-track error
x, y = current_position
cross_track_error = (a * x + b * y + c) / np.sqrt(a**2 + b**2)
# Calculate heading error
desired_heading = np.arctan2(waypoints[-1][1] - waypoints[0][1], waypoints[-1][0] - waypoints[0][0])
heading_error = desired_heading - current_heading
heading_error = (heading_error + np.pi) % (2 * np.pi) - np.pi # Normalize error to [-pi, pi]
# Compute corrections
correction_cte = np.arctan(k_ct * cross_track_error / velocity)
correction_heading = k_he * heading_error
# Compute final steering angle
steer_expect = correction_cte + correction_heading
steer_expect = np.clip(steer_expect, -1.22, 1.22) # Steering limits
return steer_expect
# Example usage
waypoints = [(1, 1), (5, 5)]
current_position = (2.5, 2)
current_heading = np.pi / 4
velocity = 3.0
steering_angle = compute_steering_correction(waypoints, current_position, current_heading, velocity)
print(f"Steering Angle Needed: {steering_angle} radians")
MPC projects the future states of the vehicle over a given time horizon and calculates the control inputs that minimize the cost function. This cost function typically includes several components to balance the objectives of path adherence, driving comfort, and dynamic response.
One thing to remember is that MPC is not an ML algorithm. It is a type of advanced control strategy that is based on optimization. While MPC and machine learning both involve computational techniques and can process data to make decisions, they operate under fundamentally different principles and are used for different purposes.
The cost function JJ in MPC can be formulated as a sum of the squared errors and control efforts over the prediction horizon. This looks like:
Detailed Components of the MPC Cost Function:
- Cross-Track Error (CTE): This error measures the lateral distance between the vehicle’s current path and the desired path. Minimizing CTE ensures the vehicle stays as close as possible to the intended trajectory.
- Heading Error: This is the difference between the vehicle’s current heading and the desired path’s direction. Reducing the heading error ensures that the vehicle is oriented correctly relative to the path.
- Control Effort: The cost function often includes terms that penalize excessive use of the steering, throttle, or brake inputs. This prevents erratic vehicle behavior and promotes smoother driving.
- Temporal Cost: Additional temporal components might include penalties for deviations from desired speeds or acceleration profiles, enhancing the comfort and safety of the vehicle’s motion.
Implementing robust longitudinal and lateral control in autonomous vehicles is a balance of theory, mathematical modeling, and careful parameter tuning. Longitudinal control benefits from a fusion of feedforward and feedback methods, enabling both anticipatory actions and corrective responses to disturbances. Lateral control strategies range from simpler geometric techniques like Pure Pursuit and the Stanley method to advanced optimization-based approaches like MPC that proactively plan ahead.
For best results, engineers must define parameters, make explicit assumptions, and acknowledge limitations. Clear documentation of variables, tuning guidelines, and practical code integration ensures that these complex control strategies are both comprehensible and actionable. As autonomous vehicle technology evolves, continued refinement in control logic and optimization approaches will drive safer, more comfortable, and more efficient travel experiences.