A C++ Linear Quadratic Regulator integrated into the ROS2 navigation
stack,
benchmarked against DWB and MPPI across six controller–planner
configurations.
The LQR controller integrates into the Nav2 stack as a
pluginlib plugin implementing
nav2_core::Controller. The implementation is
deliberately split into two files to keep pure mathematics
separate from ROS2 glue code.
The TurtleBot3 Burger is modeled as a unicycle (Dubins car). LQR is applied in three stages: body-frame error formulation, discrete linearization, and DARE-based gain computation.
State: $\mathbf{x} = [p_x,\; p_y,\; \theta]^\top$ · Control: $\mathbf{u} = [v,\; \omega]^\top$
Rather than computing error in the global frame, we project into the path's local coordinate frame. This ensures the LQR cost meaningfully penalizes lateral (cross-track) and longitudinal deviation separately:
Error vector: $\mathbf{e} = [e_{\text{long}},\; e_{\text{lat}},\; e_\theta]^\top \in \mathbb{R}^3$
Linearizing around $\mathbf{e}=\mathbf{0}$ with $v_{\text{ref}} = 0.2$ m/s, $\omega_{\text{ref}} = 0$, and applying Forward Euler with $\Delta t = 0.05$ s:
Key insight: Because $v_{\text{ref}}$ and $\omega_{\text{ref}} = 0$ are constant, $A_d$ and $B_d$ are constant — DARE is solved once at startup, not every control cycle.
Convergence: $\max|P_{k+1} - P_k| < 10^{-9}$. Typically converges in <100 iterations.
Clamped to TurtleBot3 Burger limits: $v \in [0,\; 0.22]$ m/s, $\omega \in [-2.84,\; 2.84]$ rad/s.
$q_{\text{lateral}} = 3.0$ is intentionally elevated to prioritize tight lateral path-tracking. These are the original specification gains — no tuning was performed.
These are the bugs that we have successfully identified and fixed during our implementation process. Each of these bugs was critical to resolve for the controller to function correctly and reach the goal.
The initial implementation read theta_ref directly
from the stored quarternion of each waypoint, so the reference
heading was always 0° regardless of the robot's actual position
along the path. This caused the body-frame error decomposition
to project error into a fixed global direction rather than the
local path frame, consistently producing e_long > 0 and driving
the longitudinal error term to dominate.
theta_ref geometrically
rather than from the stored quaternion:
theta_ref = atan2(next.y - ref.y, next.x - ref.x),
using the direction between adjacent waypoints on the path. This
made the body-frame decomposition correctly relative to the path
tangent and restored normal controller behavior.
MPPI's PathHandler transforms the global path into the local costmap frame and discards any waypoints that fall outside it before computing rollouts. The initial local costmap was configured as a 3×3 m window centered on the robot. For the curved NavFn path used in our benchmark, the entire planned route fell outside this window, causing MPPI to receive a path with zero poses and immediately abort with the error "Resulting plan has 0 poses in it".
The initial MPPI configuration used parameters drawn from an older API which none of them are recognized by Nav2 Humble. Critically, the configuration contained no critics list. Without critics, MPPI has no cost function and generates arbitrary trajectories with no perference for path-following.
When configuring MPPI as a benchmark baseline, the YAML
parameter files specified the controller plugin as
mppi_controller/MPPIController. However, this class does not
exist in Nav2 Humble. The correct name is
nav2_mppi_controller::MPPIController. Nav2 loads
controllers via pluginlib at runtime, so this error produced a
silent load failure rather than a compile-time error, which made
it non-obvious to diagnose.
mppi_navfn_params.yaml and
mppi_smac_params.yaml.
All runs: same Gazebo world (turtlebot3_world), same
start $(x=-2.0,\;y=-0.5)$, same goal $(x=2.0,\;y=0.5)$. Metrics
computed from rosbag recordings using odometry at ~20 Hz.
Note:
Reported Time (s) values are approximate. The benchmark
workflow is manual — we start ros2 bag record, send the
goal, and stop the bag with Ctrl+C once the robot
reaches the goal. Human reaction time on start/stop adds a few
seconds of uncertainty to each timing measurement.
| Configuration | Mean CTE (m) | Max CTE (m) | Std CTE | dv/dt | dω/dt | Time (s) | Goal |
|---|---|---|---|---|---|---|---|
| LQR + NavFn | 0.0514 m | 0.2334 m | 0.0661 m | 0.1871 | 0.9456 | 25.52 s | 3/3 |
| LQR + Smac 2D | 0.0538 m | 0.2051 m | 0.0590 m | 0.1825 | 0.4781 | 25.51 s | 3/3 |
| DWB + NavFn | 0.0489 m | 0.2568 m | 0.0772 m | 0.2998 | 1.1579 | 23.92 s | 3/3 |
| DWB + Smac 2D | 0.0475 m | 0.1990 m | 0.0586 m | 0.3110 | 1.0616 | 22.93 s | 3/3 |
| MPPI + NavFn | 0.0120 m | 0.0548 m | 0.0112 m | 0.0860 | 0.1689 | 37.65 s | 3/3 |
| MPPI + Smac 2D | 0.0807 m | 0.2846 m | 0.0772 m | 0.0965 | 0.2667 | 36.48 s | 3/3 |
Orange rows = LQR configurations. Bold = per-column best using the averaged successful runs. Goal reports successful runs out of 3.
Six simulation recordings from the benchmark campaign. Each shows
the TurtleBot3 Burger navigating from default spawn to goal $(2.0,
0.5)$ in turtlebot3_world. All six configurations
successfully reach the goal.
DWB was marginally faster than LQR — DWB+Smac reached the goal in 22.93 s vs 25.52 s for LQR+NavFn — and achieved slightly lower mean CTE (0.0475–0.0489 m vs 0.0514–0.0538 m). However, LQR produced substantially smoother commands: dv/dt 0.183–0.187 vs 0.299–0.311 for DWB, and dω/dt 0.478–0.946 vs 1.062–1.158. This confirms that LQR's optimal control formulation naturally suppresses aggressive velocity corrections that DWB's sampling heuristics permit.
MPPI+NavFn achieved the best path-tracking accuracy of all configurations — mean CTE 0.0120 m, max CTE 0.0548 m — and the smoothest velocity profile (dv/dt 0.0860, dω/dt 0.1689). However, this precision came at a 47% time penalty: MPPI+NavFn took 37.65 s vs 25.52 s for LQR+NavFn. MPPI+Smac degraded significantly (mean CTE 0.0807 m, 36.48 s), suggesting MPPI's stochastic rollouts are sensitive to path geometry. LQR offers a better speed-accuracy trade-off when consistent performance across planners is required.
LQR was nearly insensitive to planner choice — LQR+NavFn and LQR+Smac produced nearly identical results (25.52 s vs 25.51 s, mean CTE 0.0514 m vs 0.0538 m), reflecting the robustness of the body-frame error formulation. DWB+Smac was the fastest overall configuration at 22.93 s, benefiting from Smac's shorter, more direct paths. MPPI showed the largest planner sensitivity: switching from NavFn to Smac increased mean CTE by 6.7× (0.0120 m → 0.0807 m), highlighting how MPPI's rollout distribution depends heavily on path shape.