Diffusion-SAFE: Shared Autonomy Framework with
Diffusion for Safe Human-to-Robot Driving Handover

The goal of this work is to ensure safe driving performance and facilitate control transfer in collaborative human-robot driving through a closed-loop framework. This framework leverages two diffusion model-based policies:

Long-horizon policy as evaluator: It models multimodal action sequences aligned with human intent:

where it evaluates the similarity between predicted and actual human-driven trajectories.

Short-horizon policy as copilot: It models the conditional distribution of future expert action sequences:

supporting safe and efficient planning in human-in-the-loop scenarios.

To quantify human performance and determine the need for handover, we define a similarity score based on the negative log-likelihood (NLL):

The handover process is triggered when the similarity score $\mathcal{L}_{\text{NLL}}$ exceeds a predefined threshold $\tau_{NLL}$; the progressively changing forward step ratio $\gamma$ of the copilot diffusion model ensures that control is transferred seamlessly when human actions deviate significantly from safe or optimal trajectories.

Our pipeline (shown in Fig 2) uses an evaluator model that takes as input a sequence of past states, actions, and visual context to predict the next action sequence aligned with human intent. We quantify human performance and decide if control handover is needed via a negative log-likelihood (NLL)–based score. Specifically, if a potential unsafe action is detected, we modulate the forward and reverse processes of the copilot diffusion model to transition from the evaluator’s output to a sample drawn from the copilot’s expert trajectories. Once control is transferred, the copilot takes the past states and visual context as input, then samples and rolls out an expert-level action sequence. In real-world implementations, the same pipeline applies, but includes an additional step for acquiring visual context (introduced in Section IV).

We conducted our simulated experiments in CarRacing-v0, a component of the OpenAI Gym framework [15], which was originally designed for reinforcement learning and provides a simple 2D simulation in which a single car navigates through a randomly generated track. We selected CarRacing-v0 for its simplicity, adaptability, and suitability to test our shared autonomy framework, as it requires real-time human interaction; moreover, unlike other environments that simplify vehicle dynamics using a unicycle model [16], CarRacing-v0 leverages the Box2D library [17], which supports more complex and nonlinear 2D physics simulations to enhance realism. The car model we use in our work comprises five bodies: a central hull and four wheels, connected through joint constraints, enabling more accurate modeling of physical interactions and vehicle dynamics. CarRacing-v0 offers a top-down racing simulation in which the track is randomly deformed into a circular shape for each lap. It also allows users to run and collect data via a keyboard or joystick controller (introduced in Section III-F). The actions in the simulation are executed via the built-in step function of the Box2D library, which takes an action vector $\mathbf{a_{t}}=[a_{s,t},a_{th,t},a_{b,t}]^{\top}$ as input and advances the system dynamics by a fixed time step of 0.1 seconds. The single action $\mathbf{a}_{t}$ consists of three components:

• •

  Steering: A continuous action $a_{s}\in[-1,1]$, where $a_{s}=-1$ denotes a full left turn and $a_{s}=1$ a full right turn.

• •

  Acceleration: A throttle input $a_{\text{th}}\in[0,1]$, controlling the vehicle’s forward movement.

• •

  Brake: A braking input $a_{\text{b}}\in[0,1]$, where $a_{\text{b}}>0.9$ completely restricts wheel rotation.

In our work, we use two diffusion model-based policies to realize the closed-loop framework. We use DDPM [14] to approximate the conditional distribution $P_{copilot}({a}_{R,t:t_{pred}}\mid{o}_{t-t_{obs}:t})$ and $P_{eval}({a}_{H,t:t+t_{pred}}\mid{a}_{H,t-t_{obs}:t},{o}_{t-t_{obs}:t})$. In this section, we will introduce the construction of conditioning information for these two policy models.

For the policies to take into account the surrounding environment, we capture the visual context of the environment by collecting 96×96 pixel RGB images. To reduce computational complexity, we employ a lightweight Autoencoder to translate these images into a lower-dimensional latent vector $\textbf{z}\in\mathbb{R}^{128}$ (model details are provided in the code). This approach helps streamline the diffusion process and reduce both inference time and computational cost [18].

The observation information of both models at time $t$ is denoted as $\mathbf{o}_{t}=[\mathbf{s}_{t},\mathbf{z}_{t}]^{\top}$, where $\mathbf{s}_{t}$ and $\mathbf{z}_{t}$ represent the state and latent variables respectively. Each state $\mathbf{s}_{t}=[x_{t},y_{t},v_{x,t},v_{y,t},\theta_{t}]^{\top}$ encapsulates the car’s pose and velocity. For simpler subscript notation, the observation information over $t_{\text{obs}}$ steps is represented as a concatenated matrix $\mathbf{O}_{t}=[\mathbf{o}_{t-t_{\text{obs}}+1},\mathbf{o}_{t-t_{\text{obs}}+2},\dots,\mathbf{o}_{t}]$. The predicted human action sequence over $t_{\text{pred}}$ steps is represented as a concatenated matrix $\mathbf{\hat{A}}_{t}=[\mathbf{\hat{a}}_{t},\mathbf{\hat{a}}_{t+1},\dots,\mathbf{\hat{a}}_{t_{\text{pred}}-1}]$.

In this section, we provide a brief overview of the conditional diffusion model in the context of action sequence prediction, and introduce its application to shared autonomy in our work, where diffusion is utilized as a partial control mechanism.

A diffusion model captures the probability distribution by inverting a forward diffusion process, which gradually adds Gaussian noise to the intermediate distribution of an initial sample $x$. The amounts of added noise depend on a predefined variance schedule (here we choose the cosine schedule [6]). For reverse denoising process, a neural network $\boldsymbol{\epsilon}_{\theta}(\mathbf{x}^{k},k)$ takes the noisy input $\mathbf{x}^{k}$ at time step $k$ and predicts the added noisy sample $\mathbf{x}^{k}$ and the diffusion time step $k$, and learns to predict the added noise $\boldsymbol{\epsilon}$. To generate a sample from the learned distribution, we start by drawing a sample from the prior distribution $\mathbf{x}^{K}\sim\mathcal{N}(0,1)$ and iteratively denoise this sample $K$ times using $\boldsymbol{\epsilon}_{\theta}$:

where $\mathcal{N}(0,\sigma^{2}\mathbf{I})$ is Gaussian noise added back at each iteration. $\alpha$, $\gamma$, and $\sigma$ are functions of the iteration step $k$, also referred to as the noise schedule.

Moreover, diffusion models can be easily extended to model conditional distributions by adding an additional input to the denoising neural network $\boldsymbol{\epsilon}_{\theta}$. In our work, we leverage this property into the evaluator and copilot in the context of action sequence prediction. To capture conditional distributions $P_{eval}$ and $P_{copilot}$, we modify Eq. 4 to:

where $\boldsymbol{\epsilon}_{E}$ and $\boldsymbol{\epsilon}_{C}$ represent the denoising neural networks for the evaluator and the copilot policy, respectively. Notably, the evaluator conditions on both historical observations $\mathbf{O}_{t}$ and actions $\mathbf{A}_{H,t}$ (Eq. 5), whereas the copilot policy conditions only on historical observations $\mathbf{O}_{t}$ (Eq. 6). To unify the notation for later use, we denote the conditioning information for both the evaluator and copilot models as $\mathbf{C}_{t}$.

As depicted in Fig.3, our model architecture is influenced by the U-Net design with residual connections by Zagoruyko et al. [19]. Similar to the original diffusion implementations by Ho et al. [14], our architecture combines double convolution blocks (as detailed in Fig. 3) with up-sampling and down-sampling using pooling operations. These operations incorporate the conditioning information using the Feature-wise Linear Modulation (FiLM) layers [20] and the current time step after applying positional embedding.

The FiLM layers use conditioning information matrix $\mathbf{C}_{t}$ to modulate the input features $\mathbf{x}_{t}$ at each time step through dynamic scaling $\gamma$ and shifting $\beta$, defined as:

Here, $\gamma$ and $\beta$ are trainable parameters derived from $\mathbf{C}_{t}$. This enables the diffusion model to adapt its feature representation dynamically based on past observations, ensuring that the generated trajectories align with the demonstrated behavior and environmental conditions.

Since diffusion models require explicit temporal information to represent the relative position of each time step in the denoising chain, we employ positional embedding, inspired by the transformer architecture [21]. Positional embedding explicitly encodes the time step $t$ as a unique feature, enabling the model to incorporate temporal dependencies more effectively. Specifically, we use a fixed sine-cosine embedding scheme for an embedding vector of dimension which is added to the input $\mathbf{x}_{t}$, providing explicit information about the current step in the denoising process. By integrating positional embeddings, the diffusion model ensures that the denoising function $\boldsymbol{\epsilon}_{\theta}$ is aware of the sequential nature of the action generation process.

Inspired by the idea of shared autonomy as partial diffusion by Yoneda et al. [6], we modulate the forward and reverse diffusion processes in the copilot model to transform unsafe human actions into samples from the copilot expert distribution. In our work, Diffusion-SAFE leverages the concept of partial diffusion to achieve the handover process of control authority between the human and the copilot. Specifically, the forward diffusion process progressively corrupts the human distribution $P_{\text{human}}$ by introducing noise, while the reverse diffusion process refines the noisy samples to align with the safe copilot distribution $P_{\text{copilot}}$ (demonstrated in Fig 1). Here, we introduce the forward diffusion ratio $\gamma=k_{\tau}/K$, where $k_{\tau}$ is the forward steps in the inference stage of the copilot model. This ratio determines the number of forward diffusion steps applied to the human action $a_{H}$. By changing $\gamma$, we can adjust the balance between preserving human input and following the safe behavior of the copilot: when $\gamma$ is small, human intent is well preserved, and thus with limited alignment to $P_{\text{copilot}}$; in contrast, larger values $\gamma$ would lead the system to prioritize aligning with the copilot policy over human input.

To progressively transition control from the human to the copilot, we dynamically modulate $\gamma$ from $\gamma_{0}$ to 1, where $\gamma_{0}$ is the critical threshold determined empirically. To find the optimal $\gamma_{0}$, we evaluate the handover process based on smoothness and unsafe rate. Specifically, smoothness is measured by the variance of velocity, which quantifies the deviation of velocity from its mean over time:

where $v_{i}$ is the velocity at time step $i$, $\bar{v}$ is the mean velocity over the time period, $N$ is the total number of time steps. A lower variance indicates a more stable and smoother velocity profile. The unsafe rate is defined as the proportion of trials in which the vehicle goes off-road or collides with obstacles during the handover process:

where $U$ is the unsafe rate, $N_{\text{unsafe}}$ is the number of trials where the vehicle goes off-road or collides with obstacles, $N_{\text{total}}$ is the total number of trials. By jointly considering smoothness $\sigma_{v}^{2}$ and the unsafe rate $U$, we select $\gamma_{0}=0.4$ based on the performance analysis shown in Fig. 5.

Based on the CarRacing-v0 environment introduced in Section III-C, we collected an expert driving dataset $\mathcal{D}_{\text{copilot}}$ and a human driving dataset $\mathcal{D}_{\text{eval}}$ through demonstrations performed by different individuals using Xbox controllers. The Box2D physics engine generates a random closed-loop track for each lap, with variations in curvature and the number of turns upon every reset (shown in Fig. 4). Top-down RGB image sequences, joystick and trigger events (mapped to steering, throttle, and brake actions), and state sequences (including translation, orientation, and velocity) were recorded into Zarr files as human driving dataset. The expert driving dataset is recorded in a similar manner, but without the action component. The dataset serves as the foundation for training both the Autoencoder and the diffusion models.

We first train a lightweight Autoencoder to reconstruct input images by minimizing the MSE loss between the input RGB image and the reconstructed output. The encoder compresses the input into a latent vector $\mathbf{z_{t}}\in\mathbb{R}^{128}$, while the decoder reconstructs the original image using transposed convolutions. Once trained, only the encoder is used to extract the latent vector, which serves as the visual representation of the conditioning information and is subsequently utilized by the diffusion model. Note that Autoencoder is trained on the image component of the demonstration dataset.

The evaluator and copilot are trained on different dataset for different purposes. The evaluator policy is learned from ‘average human demonstrations’ dataset, denoted as $\mathcal{D}_{\text{eval}}$, which include both typical and non-positive examples such as minor off-road incidents or collisions. We assume these demonstrations, while involving occasional deviations, follow discernible patterns and exclude erratic, nonsensical behaviors. This enables the evaluator to capture human intent and predict action sequences aligned with their behavioral patterns. In contrast, the copilot policy is trained exclusively on safe ‘expert demonstrations’, denoted as $\mathcal{D}_{\text{copilot}}$, comprising suboptimal and optimal examples. As a result, the evaluator focuses on learning and mimicking human behavior, including imperfections, while the copilot specializes in generating safe and reliable action sequences. This training approach ensures that both models fulfill distinct but synergistic roles.

Another key difference between the evaluator and copilot models lies in their observation and future horizons. The evaluator policy, designed to infer human intent from demonstrations, benefits from long-horizon predictions. In contrast, the copilot policy prioritizes short-horizon predictions to achieve faster and more stable action sequences, given the computational expense of diffusion models. Both models are trained by minimizing the evidence lower bound (ELBO) of the loss function at each time step. For both models, we conducted ablation studies on the observation and prediction horizon in Section V-B.

We conducted real-world experiments on a ROS-based race car equipped with a Jetson Orin Nano for onboard processing (shown in Fig. 7). A Windows11 computer (Intel Core i9-9900KS CPU, 64GB RAM) running Motive software streamed data from a 13-camera OptiTrack motion capture system. To localize the car in the world coordinate system, four motion capture markers were selected in Motive to define a rigid body representing the car. For data transmission and network communication, the Virtual-Reality Peripheral Network (VRPN) was used to link Motive and ROS. The VRPN server on the Motive host computer transmitted pose data (2D translation and 1D orientation) as network packets to the ROS client.

To simplify the setup and avoid mounting a physical camera on the car, we captured a single top-down photo of the entire track. Using pose data from the motion capture system, we cropped real-time image patches from the top-down photo to simulate the car’s perspective. The detailed pipeline is shown in Fig. 7.

We first printed four AprilTags[22] and placed them on the ground, each with a motion capture marker attached to one of its corners. Using the motion capture system, we obtained the positions of these markers in the world frame. Next, we used the AprilTag library to retrieve the pixel coordinates of the corners in the image. Finally, we used OpenCV’s getPerspectiveTransform function, which takes four corresponding points in the world frame (from the motion capture system) and the pixel frame (from the AprilTag detection) to compute the perspective matrix $\mathbf{H}\in\mathbb{R}^{3\times 3}$, which maps the world coordinates to the pixel coordinates. To create a top-down perspective similar to the simulation environment setup, we implemented a ROS node to crop image patches $60\times 60$ from the global photo as visual information in real time. The pose of the $60\times 60$ bounding box in the pixel frame is denoted as $\mathbf{p^{pixel}_{t}}=[x_{t},y_{t},\theta_{t}]^{\top}$. Given the world coordinates from the motion capture system $\mathbf{p^{world}_{t}}=[x_{t},y_{t},\theta_{t}]^{\top}$, the corresponding pixel coordinates can be computed as $\mathbf{p^{pixel}_{t}}=\mathbf{H}\mathbf{p^{world}_{t}}$. During real-time deployment, the motion capture system streams the car’s position and orientation data, which we access via the VRPN client. Specifically, $\mathbf{p^{world}_{t}}=[x_{t},y_{t},\theta_{t}]^{\top}$ represents the bottom-center of the bounding box as Fig. 7 shown, with $\theta_{t}$ as the rotation angle used for cropping.

The frequency of the motion capture system is 100Hz while that of the control signals from joystick is 10Hz. To address the different frequencies of the motion capture system and the joystick controller, we implemented a ROS node to align the nearest corresponding actions from the controller with the poses from the motion capture system. Specifically, the node identifies the closest pose data from motion capture system and matches it with the corresponding action data from the joystick controller commands. The synchronized data are then packaged into a message and published on a ROS topic.

To collect real-world data and deploy the framework pipeline in a physical setting, we adopt the Zarr structure described in the CarRacing-v0 introduced in Section III-F. The car’s steering, throttle, and brake data are recorded as actions in the Zarr files, while synchronized image and pose data from the motion capture system are recorded as images and states respectively. Datasets are collected through demonstrations conducted by different individuals using Xbox controllers. In practice, we generated a variety of racing tracks (with combinations of different turns and obstacles) using green tape as track bounds and red tape as obstacles (illustrated in Fig. 4). Consistent with the simulation training logistics, the Autoencoder’s training dataset is derived from the image component of the demonstration dataset. Real-world examples are shown in Fig. 8.

To comprehensively evaluate the predicted trajectories, we introduce three metrics for the copilot diffusion model and the evaluator diffusion model, adapted from the nuPlan benchmark metrics [23]. These metrics are carefully designed to capture the multifaceted aspects of trajectory prediction.

We perform an ablation study to evaluate the effectiveness of model components with results summarized in Table I and II, with the best model (‘ours’) demonstrated in the real world. These studies were performed on both the evaluator and the copilot model. Specifically, we analyzed the impact of position, visual context, and observation & prediction horizons on the performance of these models. In addition, we examined how the incorporation of the past human action sequence impacts the effectiveness of the evaluator model. The unit of horizon is the step (0.1s).

We utilize the ability of the diffusion policy to inherently express multimodal distributions. Our method is compared to the following multimodal methods: LSTM-GMM [24] and Behavior Transformers models (BET) [25]. The results are summarized in Tables III and IV.

To evaluate our partial diffusion method in the handover process, we compare it to a simple blending method:

where $\mathbf{a_{\text{blend}}}$ is the blended action, $k$ is the blending coefficient which decreases over time to gradually shift control from human to the copilot, $\mathbf{a_{H}}$ is the action of human, and $\mathbf{a_{\text{C}}}$ is the action of the copilot. Fig. 9 shows that the simple blending method leads to unsafe off-road states, while our method safely completes the handover process. Notice that even after the simple blending method leads the vehicle to unsafe states, the copilot is able to correct it back to the track.