The factory linear Adaptive cruise controller (ACC) in Openpilot, Comma.ai

This post will dig into OP’s code on the adaptive cruise control (ACC) module. This method is no longer used in the current OP version. The code shared in this post is retrieved from OP 3.0.5.

commaai/openpilot

First, we quote the explanation from Comma.ai which shows how the longitudinal control was conducted in the old-school way.

Adaptive Cruise Control (longitudinal)

We separate the gas/brakes from the steering in openpilot. Longitudinal control is still done in an old school way, without a neural network.

radard is used to parse the messages from the car radar. It does a rudimentary fusion with the vision system, and outputs the positions of up to two “lead cars” at 20hz.

Then adaptivecruise uses the leads and the current speed to determine how fast you should be going.

Radar.py that generates data for ACC controller:

ACC and its functions:

Here we have an AC object, which does the planning task. The key step in planner.py is:

self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)

l20 is the message that includes the lead vehicle information.

The AC planner is defined as:

class AdaptiveCruise(object):
  def __init__(self):
    self.last_cal = 0.
    self.l1, self.l2 = None, None
    self.dead = True
  def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
    if l20 is not None:
      self.l1 = l20.live20.leadOne
      self.l2 = l20.live20.leadTwo
      # TODO: no longer has anything to do with calibration
      self.last_cal = cur_time
      self.dead = False
     elif cur_time - self.last_cal > 0.5:
      self.dead = True
     self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
     self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

The key function to compute the vTargetEgo is:

self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)

Note that self.v_target_lead is vTargetEgo that we are looking for. To avoid confusion, it is not the speed for lead vehicle.

Plan the next-step speed for Ego vehicle

It processes lead vehicle info and calculates the desired distance before calculating the desired speed for the Ego vehicle. The two processing functions, especially the desire_distance are shown below:

def process_a_lead(a_lead):
  # soft threshold of 0.5m/s^2 applied to a_lead to reject noise,     also not considered positive a_lead
  a_lead_threshold = 0.5
  a_lead = min(a_lead + a_lead_threshold, 0)
  return a_lead
def calc_desired_distance(v_lead):
  #*** compute desired distance ***
  t_gap = 1.7  # good to be far away
  d_offset = 4 # distance when at zero speed
  return d_offset + v_lead * t_gap

Then, the most critical step is to use the above lead info to calculate the desired speed for the Ego vehicle:

The logic is to compute the desired relevant speed, v_rel_des, using the desired distance. And the desired speed is just

v_target = v_rel_des+v_lead

Note that v_target is actually the v_targetEgo.

In the above code, we have seen parameters:

max runaway speed: 2 m/s²
desired distance: 4m + v_lead * 1.7s

The aggressiveness of the planner is mainly determined by the multiplier on the distance error between the desired distance and true distance.

In the decelerating case:

if d_lead < d_des: #66, if the true distance is smaller than desired 
    # calculate v_rel_des on the line that connects 0m at  max_runaway_speed to d_des
    v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
    # calculate v_rel_des on one third of the linear slope
    v_rel_des_2 = (d_lead - d_des) * l_slope / 3.

# interpolate the lookups to find the slopes for a given lead speed. Actually, it can be viewed as a proportional gain. It means the gain is interpolated using current speed.

 l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
 p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)

# this is where parabola and linear curves are tangents

x_linear_to_parabola = p_slope / l_slope**2

# parabola offset to have the parabola being tangent to the linear curve

x_parabola_offset = p_slope / (2 * l_slope**2)

In the accelerating case where the Ego vehicle needs to catch up:

v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)

The governing parameters of the ACC controller are predefined and fixed:

#linear slope
_L_SLOPE_V = [0.40, 0.10] #66, tunable
_L_SLOPE_BP = [0., 40]
# parabola slope
_P_SLOPE_V = [1.0, 0.25] #66, tunable
_P_SLOPE_BP = [0., 40]

We can definitely tune those parameters to change the string stability of the AC controller. However, here we do not account the PI controller to execute the vEgo_target using brake and gas.

Notice that we also have considered the acceleration bounds:

def calc_cruise_accel_limits(v_ego):
    a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
    a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max])

The acceleration bounds are interpolated using current speed, which is a similar way to our bounded acceleration model, but no longer linear.

# lookup tables VS speed to determine min and max accels in cruise
_A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.] # accelerations
# need fast accel at very low speed for stop and go
_A_CRUISE_MAX_V  = [1., 1., .8, .5, .30]
_A_CRUISE_MAX_BP = [0.,  5., 10., 20., 40.]

Notice that when speed is at 40m/s, the max acceleration is 0.3 1.0 m/s², and the maximum acceleration for speed zero is 1.0 m/s^2.

Those parameters would greatly impact the performance in stop-and-go traffic.

# *** gas/brake PID loop ***

final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, self.plan.vTarget, [self.plan.aTargetMin, self.plan.aTargetMax], self.plan.jerkFactor, self.CP)

Now we show the longitudinal controller for executing the plan from ACC:

Note that a_target is a list, which includes the min and max of acceleration. They are used to compute the upper and lower bounds for the v_pid in the PID controller.