Model Predictive Control with Visibility Graphs for Humanoid Path Planning and Tracking Against Adversarial Opponents

TL;DR

This work presents DAVG, a dynamics- and turning-aware path-planning framework built on augmented visibility graphs, combined with cf-MPC to track trajectories for a humanoid robot in RoboCup soccer. DAVG efficiently identifies a small, active set of obstacles and augments edge costs with turning-angle penalties to favor smoother paths, while cf-MPC integrates collision-avoidance constraints into a single optimization framework without mode-switching. The nonlinear and linear MPC formulations (NMPC/LMPC) balance obstacle avoidance and robot dynamics, including a tangent-plane linearization of obstacle constraints to ensure feasibility. The approach achieves real-time performance (NMPC ~120 Hz, LMPC ~400 Hz) and demonstrated success on hardware, enabling robust long-term planning and responsive short-term tracking in a dynamic, noisy environment. Overall, the combination of DAVG and cf-MPC enabled competitive RoboCup 2024 results, with demonstrated online re-planning and obstacle handling under limited FOV and measurement noise.

Abstract

In this paper we detail the methods used for obstacle avoidance, path planning, and trajectory tracking that helped us win the adult-sized, autonomous humanoid soccer league in RoboCup 2024. Our team was undefeated for all seated matches and scored 45 goals over 6 games, winning the championship game 6 to 1. During the competition, a major challenge for collision avoidance was the measurement noise coming from bipedal locomotion and a limited field of view (FOV). Furthermore, obstacles would sporadically jump in and out of our planned trajectory. At times our estimator would place our robot inside a hard constraint. Any planner in this competition must also be be computationally efficient enough to re-plan and react in real time. This motivated our approach to trajectory generation and tracking. In many scenarios long-term and short-term planning is needed. To efficiently find a long-term general path that avoids all obstacles we developed DAVG (Dynamic Augmented Visibility Graphs). DAVG focuses on essential path planning by setting certain regions to be active based on obstacles and the desired goal pose. By augmenting the states in the graph, turning angles are considered, which is crucial for a large soccer playing robot as turning may be more costly. A trajectory is formed by linearly interpolating between discrete points generated by DAVG. A modified version of model predictive control (MPC) is used to then track this trajectory called cf-MPC (Collision-Free MPC). This ensures short-term planning. Without having to switch formulations cf-MPC takes into account the robot dynamics and collision free constraints. Without a hard switch the control input can smoothly transition in cases where the noise places our robot inside a constraint boundary. The nonlinear formulation runs at approximately 120 Hz, while the quadratic version achieves around 400 Hz.

Figures (14)

• Figure 1: Top figure shows a situation where our robot, ARTEMIS, in red is trying to avoid the opponent in blue to get to the ball during the championship match in RoboCup 2024. There are many situations like this or even worse when trying to reach the ball. Bottom figure gives an overview of our entire framework for trajectory generation and tracking. $P_s$ and $P_g$ represent the starting pose and goal pose, respectively. $P_{obs,j}$ and $R_{obs,j}$ represent the position and radius of obstacles, respectively.
• Figure 2: Regular polygon representation of obstacles.
• Figure 3: Process for selecting active obstacles and active regions.
• Figure 4: Illustration of selecting active region procedure.
• Figure 5: How to define the edge when the starting point or ending point is covered by an obstacle.