MPC Tracking Layer for TEB Controller

[2006wu]

Description

Currently, the navigation pipeline uses TEB to generate /cmd_vel:

Planner
   │
   ▼
Global Path
   │
   ▼
TEB Controller
   │
   ▼
 /cmd_vel
   │
   ▼
Robot

At high speed, the actual robot motion may not perfectly follow the planned trajectory because of inertia, acceleration limits, and tracking errors.

Nav2 provides the MPPI Controller, which uses model predictive control concepts. However, previous testing showed that it was difficult to tune for both high-speed motion and responsive obstacle avoidance, so TEB is currently still used as the main controller.

This feature aims to investigate adding a lightweight MPC tracking layer after TEB, instead of replacing TEB completely.

---

MPC Tracking

The MPC tracker receives the desired motion from TEB and uses the current robot state from odometry to predict future motion.

Planner
   │
   ▼
Global Path
   │
   ▼
TEB Controller
   │
   ├── desired_cmd_vel
   │
   └── trajectory / reference
   │
   ▼
MPC Tracker ◄──── /odom
   │
   ▼
 corrected_cmd_vel
   │
   ▼
 /cmd_vel
   │
   ▼
Robot

The MPC should minimize tracking error while considering constraints such as:

• Position / trajectory tracking error
• Velocity tracking error
• Linear and angular velocity limits
• Acceleration limits
• Control smoothness

Conceptually:

Current State + Reference
          │
          ▼
   Predict Future Motion
          │
          ▼
    Optimize Control
          │
          ▼
 Corrected /cmd_vel

The goal is to keep TEB responsible for local planning and obstacle avoidance, while MPC focuses on motion prediction and trajectory tracking.

---

MPC Correction Limit

To prevent MPC from changing the TEB command too aggressively, the MPC output should be treated as a limited correction to the original TEB command.

u_mpc = u_teb + Δu
|Δu| <= Δu_max

This keeps MPC focused on tracking correction without excessively changing the motion command generated by TEB.

---

Controller Structure

Possible structure:

custom_controller/
├── include/
│   ├── custom_controller.hpp
│   ├── teb_controller.hpp
│   └── mpc_tracker.hpp
│
└── src/
    ├── custom_controller.cpp
    ├── teb_controller.cpp
    └── mpc_tracker.cpp

The exact architecture can be adjusted during implementation.
The MPC tracker receives the desired motion and trajectory reference from TEB, and uses the current robot state from odometry to predict future motion.

---

Optional MPC Mode

The MPC tracking layer should be configurable so that TEB can still run independently.

For example:

use_mpc_tracker: true  # true: TEB + MPC, false: TEB only

use_mpc_tracker = false

TEB → /cmd_vel

use_mpc_tracker = true

TEB ── desired_cmd_vel ──┐
                         ├──→ MPC → /cmd_vel
TEB ── trajectory ───────┘       ▲
                                 │
                               /odom

This makes it easier to compare the original TEB controller against TEB + MPC.

---

Expected Behavior

• Reduce trajectory tracking error at high speed
• Improve tracking during high-speed turning
• Keep TEB responsible for obstacle avoidance
• Respect velocity and acceleration constraints
• Allow MPC correction to be enabled or disabled

---

Comparison

Compare:

TEB vs. TEB + MPC Tracker

Evaluate:

• Path tracking error
• High-speed turning performance
• Obstacle avoidance behavior
• Velocity smoothness

The main goal is to determine whether adding MPC as a tracking layer can improve high-speed trajectory tracking without losing the obstacle avoidance behavior of TEB.

---

Tasks

• Study MPC tracking and define the robot motion model
• Implement mpc_tracker with /odom state feedback and control constraints
• Modify TEB to provide desired_cmd_vel and trajectory reference to MPC
• Integrate TEB and MPC and verify the command pipeline
• Test and tune MPC locally
• Test and tune on the actual machines and evaluate the results
• Discuss the results and decide the next step