$ \DeclareMathOperator{\DOT}{.} \DeclareMathOperator{\nondet}{|} \newcommand\conc{\parallel} \newcommand\concNarrow{\!\parallel\!} \newcommand\D{\mathcal{D}} \newcommand\C{\mathcal{C}} \newcommand\R{\mathcal{R}} \newcommand\bigO{\mathcal{O}} \newcommand\rew{{r}} \newcommand\subwedge{\,\wedge\,} \newcommand\submid{\,\mid\,} \newcommand\Tau{\mathrm{T}} \newcommand\defeq{\stackrel{\text{\tiny\sf def}}{=}} \newcommand\ang[1]{\ensuremath{#1^\circ}} \newcommand\act[1]{\mathsf{#1}} \newcommand\N[1]{\mathit{#1}} $

Plan Recognition by Program Execution in Continuous Temporal Domains

Knowledge-Based Systems Group

RWTH Aachen University

Cognitive Robotics Workshop 2012

Motivation

What?
- plan recognition
- automobile traffic
Why?
- continuous world
- multiple agents
- no action sensor
How?
- pre-defined set of programs = typical behavior
- simulate program execution
- match simulation and reality

Passing Maneuvers

Real: cont., noisy

Observed: discrete, noisy

Model: cont., instantaneous

Reality:: continuous movement, continuous actions, noise
Observations:: discrete sequence of snapshots, noisy
Model:: continuous movement, instantaneous actions, no noise

Actions are not observable directly
Multiple agents act in parallel

Foundations

Situation Calculus
- actions are instantaneous
- situations $S_0$, $do(a, s)$
- fluents = situation-dependent predicates / functions
- preconditions $Poss(a, s)$
Golog
- action language
- nondeterminism allows different executions
- programs like
  
  $\pi \theta$ . setYaw($\theta$);
  
  waitFor(onTurningLane);
  
  if redLight then
  
  setVeloc(0)

Car Model

Actions:
$setVeloc(car, v, \tau)$

$setYaw(car, \theta, \tau)$
Fluents:
$veloc(car, s) = v$

$yaw(car, s) = \theta$
longitude:
$x(car, s) = linear(x_0, \cos \theta \cdot v, \tau)$
latitude:
$y(car, s) = linear(y_0, \sin \theta \cdot v, \tau)$
$linear(x_0, a, \tau)$ stands for $f(\tau') = x_0 + a \cdot (\tau' - \tau)$

Car Model

Time:

Actions are instantaneous, but change is continuous (X, Y position)
Time is encoded as (last) parameter in actions
A left-lane-change that starts after 3 and ends after 6.1 seconds:
do(setYaw(P, 0.0°, 6.1), do(setYaw(P, -8.9°, 3.0), S₀))

Sample Programs

proc leftLaneChange($car_1$)

$\pi \theta$ . ($0° < \theta \leq 90°$)?;

$\pi \tau_1$ . setYaw($car_1$, $\theta$, $\tau_1$);

$\pi \tau_2$ . waitFor(onLeftLane($car_1$), $\tau_2$)

setYaw($car_1$, $0°$, $\tau_2$);

proc overtake($car_1$, $car_2$)

$\pi \tau_1$ . $\pi v$ . setVeloc($car_1$, $v$, $\tau_1$)

$\parallel$

leftLaneChange($car_1$);

$\pi \tau_2$ . waitFor(behind($car_2$, $car_1$), $\tau_2$);

rightLaneChange($car_1$)

Semantics of our Golog

Continuous change
ccGolog
Decision theory for online execution
DTGolog
- reward function $r(s)$ rates situations
- resolve nondeterminism to reach $r$-maximizing situation after $\ell$ actions
Concurrency $\delta_1 \parallel \delta_2$ for multiple agents acting in parallel
ConGolog
Stochastic actions for robustness (later)
stGolog, DTGolog

Plan Recognition

Translate observation $\phi$ at time $\tau$ as action $match(\phi, \tau)$ with \[Poss(match(\phi, \tau), s) \equiv \phi[s, \tau] \]
Reward $r(s) =$ number of $match$ actions in $s$
$\Rightarrow$ interpreter tries to explain observations
e.g., $\pi \theta$ . setYaw($car_1$, $\theta$, $\tau_1$) picks a $\theta$ that matches observations

Plan Recognition

Test hypothesis $\delta_1 \parallel \ldots \parallel \delta_n$ for agents $1, \ldots, n$

$s := S_0$, $\delta := \delta_1 \parallel \ldots \parallel \delta_n$

Loop

If $\delta$ has $< \ell$ (look-ahead) $match$ actions then

wait for $\phi, \tau$ and set $\delta := \delta \parallel match(\phi, \tau)$
Else if $transPr(\delta, s, \delta', s') > 0$ (next action executable) then

set $\delta := \delta'$, $s := s'$
Else

fail and reject hypothesis

EndLoop

Plan Recognition

Multiple agents (R, P) act in parallel
$\ell$ observations are buffered to resolve nondeterminism reasonably
Incremental execution of the program

$\pi \tau_1$ . $\pi v_1$ . setVeloc(R, $v_1$, $\tau_1$)

||

        $\pi \tau_2$ . $\pi v_2$ . setVeloc(P, $v_2$, $\tau_2$);
$\pi \tau_3$ . $\pi \theta_1$ . setYaw(P, $\theta_1$, $\tau_3$);
$\pi \tau_4$ . waitFor(onLeftLane(P), $\tau_4$);
setYaw(P, 0, $\tau_4$);
$\pi \tau_5$ . waitFor(behind(R, P), $\tau_5$);
$\pi \tau_6$ . $\pi \theta_2$ . setYaw(P, $\theta_2$, $\tau_6$);
$\pi \tau_7$ . waitFor(onRightLane(P), $\tau_7$);
setYaw(P, 0, $\tau_7$)

...

Robustness

People don't drive exactly straight $\rightarrow$ be tolerant!
- well-rested $\rightarrow$ very straight
- tired $\rightarrow$ oscillates
- drunk $\rightarrow$ hardly stays on the road

setYaw($car$, $\theta$, $\tau$) stochastic action with outcomes setYaw$'$($car$, $\theta$, $\Delta$, $\tau$)
Large tolerances $\Delta$ are unlikely
Confidence $=$ sum of prob. of outcome actions with tolerances $\Delta$ big enough for the observations

Demo

Aggressive

Cautious

Three cars in the road
- A on the right lane,
- B on the left lane,
- C wants to pass A
Two options for C:

Limitations in experiment:

Acceleration approximated by partially linear functions
Velocities and "accelerations" hard-coded

Evaluation

Passing maneuver
- single hypothesis, system needs to confirm or reject
- 6 drivers, 120 maneuvers
- no false positive, no false negatives

Cautious vs aggressive maneuver

two alternative hypotheses, system needs to choose
2 drivers, 24 maneuvers

recognition confidences:

		reality
		cautious	aggressive
hypothesis	cautious	0.3 0	0.0 0
hypothesis	aggressive	0.0 0	0.57

New implementation has lower latency

Conclusion

Programs capture typical behavior
Execute programs in sync with observations
- online
- multi-agent
- continuous
- robust
- no action sensor

Future Work:

More realistic models
- longitudinal robustness?
- nonlinear / qualitative motion?
Incomplete knowledge
Extrapolate program $\rightarrow$ future situation