Skip to content
ELECTRONICS ENGINEERINGelectronics-engineering.com
Automotive

ACC 4: target selection, path prediction and false positives

Path from yaw rate and lane lines fused with their uncertainty, in-path probability proved, false positives in curves, stationary objects, state machine, predicted cut-in.

https://www.electronics-engineering.com/en/acc-target-selection-path-prediction-cut-in-false-positives/

No ratings yet: be the first to rate this page.

Fusion gives a list of objects: cars in our lane, cars in the neighbouring lanes, guard rails, signs, sometimes a ghost echo. Target selection chooses which one to follow. There are two possible mistakes and they weigh differently: a false positive (braking for a car that is in the next lane) is annoying and dangerous, because whoever follows does not expect it; a false negative (not seeing in time a car that enters our lane) forces hard braking at the last moment. The heart of the problem is predicting where our own car will go, even when the lines on the road are poorly visible.

Where we will go: the predicted path

From the yaw rate

A car driving through a curve of radius R at speed v rotates with angular velocity \dot\psi = v/R, so the curvature is

    \[ \kappa = \frac{1}{R} = \frac{\dot\psi}{v}. \]

The gyroscope measures \dot\psi always, in any weather. If the curvature stays as it is now, the path is a circle: the point at distance x ahead is at lateral position y = R - \sqrt{R^2 - x^2}. For x \ll R, with the expansion \sqrt{1-u} \approx 1 - u/2:

    \[ y = R\left(1 - \sqrt{1 - x^2/R^2}\right) \approx \frac{x^2}{2R} = \frac{\kappa x^2}{2}. \]

But the curvature changes. Roads are built with transition curves whose curvature varies linearly with distance (clothoids), \kappa(s) = \kappa_0 + c\,s. The heading angle is \int\kappa\,ds = \kappa_0 s + c s^2/2 and, integrating again, the lateral displacement is

    \[ y(x) \approx \frac{\kappa_0 x^2}{2} + \frac{c\,x^3}{6}. \]

The gyroscope sees \kappa_0 but not c: at the entry of a curve our car still drives straight while the road already turns, and the error grows with x^3. In the model we treat c as uncertain (standard deviation \sigma_c = 4\cdot 10^{-6} m⁻², typical of motorway transition curves) and the gyroscope noise as uncertainty on \kappa_0: the uncertainty of the path grows quickly with distance.

From the lane lines

The camera gives the centre of the lane as a polynomial y = c_0 + c_1 x + c_2 x^2 + c_3 x^3, with a quality q \in [0,1] (contrast, both lines visible, line type) and a view range (how far the lines are seen). The lines “see” the curve before we enter it, but they can be missing: road works, snow, worn lines, junctions. Their error grows with distance and becomes huge beyond the view range.

Fusing the two paths

Each point x has two estimates with their standard deviations, y_{\dot\psi}(x),\ \sigma_{\dot\psi}(x) and y_l(x),\ \sigma_l(x). They are combined like two independent measurements (the same proof as in lesson 3): weights inversely proportional to the variances, 1/\sigma_p^2 = 1/\sigma_{\dot\psi}^2 + 1/\sigma_l^2. With good lines the camera wins; with poor lines the yaw rate remains, with an uncertainty that honestly says how little we know.

Predicted path at the entry of a curve
At the entry of a left curve (it starts 50 m ahead). The dotted line is the true lane centre, the grey one the path from the yaw rate alone (still straight). Left, good lines (q = 0.9, view range 110 m): the fused path (blue, with the band \pm\sigma_p) follows the curve, at 110 m 0.73 ± 0.33 m against a true 0.74. Right, poor lines (q = 0.15, 30 m): the path stays straight but the band widens, ±2.7 m at 110 m. The red square is a car in the right lane.

The probability of being on our path

Path corridor and lateral offset
The corridor of half width w around the predicted path y_p(x) and an object with lateral offset \delta. The light blue band represents the uncertainty \sigma_p(x), which grows with distance.

For an object at distance x and lateral position y, the offset from the path is \delta = y - y_p(x). The true offset is uncertain: adding the uncertainty of the object (from the tracker covariance) and that of the path, \sigma^2 = \sigma_{y,obj}^2 + \sigma_p(x)^2. The object is on our path if the true offset lies within the corridor |\delta_{true}| \mathchar"313C  w; with a Gaussian offset of mean \delta, the probability is the integral of the Gaussian between -w and w:

    \[ P_{in} = \Phi\!\left(\frac{w - \delta}{\sigma}\right) - \Phi\!\left(\frac{-w - \delta}{\sigma}\right), \]

with \Phi the cumulative distribution function of the standard normal. The half width w = 1.1 m is that of our car plus a margin, not that of the lane: what matters is whether the other vehicle occupies the space we will sweep, not where the lines are.

Probability of being on the path
P_{in} as a function of the offset \delta for different total uncertainties. With \sigma = 0.15 m the curve is almost a rectangle: in or out. With \sigma = 2 m it does not exceed 0.4 even at the centre: a distant object with an uncertain path cannot become the target. The horizontal lines are the entry (0.6) and exit (0.35) thresholds.

This is already a defence against false positives: where we do not know where we will go, no object exceeds the threshold.

False positives and how to avoid them

The classic case: we approach a left curve; a car in the right lane, 95 m ahead, is already in the curve. Relative to our current, straight direction, its displacement brings it right in front of us: a selector that uses only the yaw rate with a fixed uncertainty takes it as the target and brakes.

False positive at the entry of a curve
The car in the right lane at 95 m as we enter the curve. Top: the offset from the predicted path; the corridor is the light blue band. Bottom: P_{in}. With the naive selector (path from the yaw rate only, fixed uncertainty, grey) the probability reaches 0.84 and the car is taken as the target for 2.2 s (shaded area): braking for nothing. With poor lines but honest uncertainty (orange) it reaches 0.44: never a target. With good lines (green) it stays at 0.

The defences, all in the code:

  1. path with honest uncertainty: the uncertainty grows with x^2 and x^3; with poor lines a distant object does not reach the threshold;
  2. no new targets where the path is too uncertain: if \sigma_p \mathchar"313E  2 m at the object’s distance, the object cannot become a candidate (a target already locked is kept, with hysteresis);
  3. confirmation time that grows with uncertainty: a candidate must stay above the threshold for \tau_c = 0.3 + 0.25\,\sigma_p seconds;
  4. hysteresis: entry with P_{in} \mathchar"313E  0.6, exit with P_{in} \mathchar"313C  0.35: no target that appears and disappears;
  5. stationary objects: for the radar they are all the same (lesson 2). A stationary object is admitted only if it has been seen moving (a car that stopped in a queue) or if the camera has confirmed it within the last 0.5 s and it is within 80 m;
  6. confirmed tracks only: isolated echoes never get here (M-of-N logic of lesson 3).

The selection state machine

Target selection state machine
NONE: no target. CANDIDATE: an admitted object exceeds p_{in}; it must stay there for \tau_c(\sigma_p). LOCKED: it is the target the planner follows; if a closer object becomes a confirmed candidate it replaces it (cut-in); if P_{in} falls below p_{out} it is released (cut-out). HOLD: the target track was lost for a moment; its prediction is kept for \tau_h, then NONE.
FromToCondition
NONECANDIDATEclosest admitted object with P_{in} \mathchar"313E  0.6 and \sigma_p \mathchar"313C  2 m
CANDIDATENONEthe candidate drops below 0.6 or disappears
CANDIDATELOCKEDit stays above the threshold for \tau_c = 0.3 + 0.25\,\sigma_p s
LOCKEDLOCKED (new)a closer object is a candidate for \tau_c: cut-in
LOCKEDNONEP_{in} \mathchar"313C  0.35 or object no longer admitted: cut-out
LOCKEDHOLDthe target track is missing (lost measurements, occlusion)
HOLDLOCKEDthe track comes back
HOLDNONE0.6 s have passed

The HOLD state looks like a detail, but without it every time the radar loses the target for a few cycles (it happens, for example, under a bridge) the ACC would accelerate towards the set speed and then brake again. In lesson 7 a 0.4 s loss of radar measurements passes without a single cycle without a target.

The cut-in: predicting who comes in

A vehicle entering our lane
A vehicle in the next lane moves towards us with lateral speed v_y. The time to line crossing, TLC, is the offset still to cover divided by the lateral speed.

Waiting until the car is already in the corridor means reacting late. If an object outside the corridor is moving towards it, we also compute where it will be in t_p = 1 s, \delta_p = \delta + v_y t_p, with an uncertainty increased by the variance of the lateral speed (\sigma_p^2 = \sigma^2 + t_p^2\sigma_{v_y}^2), and we use the higher of the current and the predicted probability (the latter multiplied by 0.9: a prediction is worth a little less than a measurement).

Cut-in with and without prediction
Simulation with the whole chain (sensors, fusion, selection, planning, torque, vehicle): a car 12 km/h slower enters the lane 50 m ahead. From the top: lateral position, P_{in}, acceleration of our car, selection state. With prediction (violet) the target is locked at 3.92 s, when the car is still 1.57 m from the centre; without (grey) at 4.78 s. Result: peak braking 3.0 m/s² instead of 3.7, minimum distance 34.0 m instead of 31.5.

The code

Here is the evaluation of an object in the C version (the Python one is identical):

static bool ts_assess(TgtSel *s, const AccObject *o, const AccPath *path, float v_ego, float t, float *prob, float *sp)
{
    float yp, sig, delta, v_abs;
    bool stationary, ok;
    *prob = 0.0f; *sp = 0.0f;
    if (o->x <= 0.5f) return false;
    PathPred_At(path, o->x, &yp, sp);                 /* fused path and its sigma at the object */
    delta = o->y - yp;
    sig = sqrtf(o->var_y + *sp * *sp);
    *prob = InPathProbability(delta, sig, TS_HALF);
    if (fabsf(o->vy) > 0.2f && sgnf(o->vy) == -sgnf(delta) && fabsf(delta) > TS_HALF) {
        float d_pred = delta + o->vy * TS_T_PRED;      /* cut-in: where it will be */
        float s_pred = sqrtf(sig * sig + TS_T_PRED * TS_T_PRED * o->var_vy);
        *prob = maxf(*prob, 0.9f * InPathProbability(d_pred, s_pred, TS_HALF));
    }
    v_abs = v_ego + o->vx;
    stationary = fabsf(v_abs) < 1.0f && !was_moving(s, o->id, fabsf(v_abs) > 3.0f);
    ok = o->confirmed;
    if (stationary)       /* stationary: camera confirmation and close range */
        ok = ok && (t - o->t_cam < 0.5f) && o->x < TS_X_STAT;
    return ok;
}

Now we know which vehicle to follow, its distance d, its speed v_t and its acceleration a_t. In the next lesson we decide how to follow it: how much to accelerate or brake, cycle after cycle.

Subscribe
Notify of
guest
0 Comments
Oldest
Newest Most Voted
0
Would love your thoughts, please comment.x
()
x