WATonmous Self Driving Design Team

In early 2024, I joined WATonomous, Waterloo's autonomous driving team, as a Machine Learning developer on the action team. I started work by collaborating on the development of a parallel solver for real-time trajectory planning with static and dynamic obstacles. This solver rapidly generates safe driving paths by simultaneously evaluating multiple potential trajectories in parallel. It first represents the vehicle's planned route mathematically, then efficiently checks each possible path for collisions against surrounding obstacles. The solver concurrently assesses many trajectory candidates, quickly discarding unsafe ones and selecting optimal, collision-free paths.

Data Driven Model

I eventually switchd to working on a data driven model. Consider a self-driving car that must continually plan its trajectory, stay on the road, and avoid obstacles. One effective method to manage this control problem is Model Predictive Control (MPC). MPC works by repeatedly solving an optimization problem, looking ahead over a short future horizon and selecting the best immediate action.

Mathematically, MPC solves:

$$\min_{u_{0}, \dots, u_{N-1}} \sum_{k=0}^{N-1} L(x_k, u_k)$$

subject to:

$$x_{k+1} = \phi(x_k, u_k, \delta t), \quad x_0 \text{ given}, \quad g(x_k, u_k) \leq 0.$$

Here, $(x_k)$ represents the state of the car (position, velocity, orientation), $(u_k)$ are control inputs (steering, throttle, brake), and $L(x_k, u_k)$ evaluates the quality of the actions (e.g., staying in the lane, avoiding collisions). Constraints $g(x_k, u_k)$ ensure safety and feasibility.

Typically, a simplified physics-based dynamics model (like a bicycle model) predicts the car's future state. However, these models lack complexity—ignoring factors like tire friction, road surface irregularities, or wind resistance. To capture these intricate behaviors, we add a learned correction using real-world data:

$$f(x,u) = f_F(x,u) + f_D(x,u).$$

The physics-based $f_F$ provides the base dynamics, and $f_D$, a neural network trained from real driving data, captures additional nuanced effects.

Our neural network was trained using collected driving data. The input features included vehicle state (position, orientation quaternions), velocity vectors, rotation rates, and control inputs. We also incorporated additional spatial features using localized 3×3 ground effect patches around the vehicle's position to further enhance accuracy. This approach allowed the model to account for terrain variations and surface conditions that affect vehicle dynamics. For the ground effect features, we mapped the vehicle's position to a discretized grid and extracted surrounding terrain information, concatenating this with orientation data to provide spatial context.

The model was implemented as a normalized MLP with tanh activation functions, where both input and output normalization were handled within the model itself. This architecture maintained the raw physics in the forward model while learning the residual dynamics from data.

However, directly using a complex neural network within the MPC loop can be computationally demanding. Based on the paper Real-time Neural-MPC, we employed local approximation, simplifying the neural network predictions around the car's current state and control input:

$$f_D^*(x,u) \approx f_D(x_i,u_i) + J_{D,i} \begin{bmatrix} x - x_i \\ u - u_i \end{bmatrix} + \frac{1}{2} \begin{bmatrix} x - x_i \\ u - u_i \end{bmatrix}^\top H_{D,i} \begin{bmatrix} x - x_i \\ u - u_i \end{bmatrix},$$

where $J_{D,i}$ (Jacobian) and $H_{D,i}$ (Hessian) are computed around the current operating point. This local approximation allows rapid computation during real-time control. Treating a neural network as function approximators, we use the principle that functions can be approximated—lirbaries like ml-casadi help differentiate these models. By using an approximation, we can create high-efficiency neural representations. The results we found was approximately got 40% better results than traditional MPC and 2x slower, while the full neural model was 70% better, but nearly 20x slower. This heavily justifies further exploration into these approximations.