Abstract
This paper presents a real-time, whole-body Model Predictive Control (MPC) framework designed for legged robots with manipulator arms. The system enables highly dynamic maneuvers while autonomously avoiding self-collisions and environmental obstacles. By integrating collision avoidance as soft constraints via efficient distance queries and signed distance fields, the researchers demonstrate a quadruped's ability to balance using its arm, perform dynamic weight throwing, and navigate through constrained environments without the need for manual heuristics or offline pre-planning.
Introduction
Legged mobile manipulators, such as quadrupeds equipped with robotic arms, offer superior agility and reachability compared to wheeled systems. However, controlling these platforms remains a significant challenge. Traditional approaches often decouple the control of the legs (locomotion) and the arm (manipulation). This "divide and conquer" strategy frequently fails during dynamic tasks—such as throwing heavy objects or traversing rough terrain—where the base and arm must move in perfect coordination to maintain stability.
While unified whole-body MPCs have been developed, they often neglect collision avoidance. Without explicit safety constraints, a robot might calculate a trajectory to reach a target but inadvertently strike its own sensors or nearby obstacles. Historically, preventing these collisions required "hand-crafted heuristics" that limited the robot's range of motion. This research seeks a unified mathematical approach to handle collisions within the optimization loop, preserving safety without sacrificing dynamic performance.
Methodology
The proposed framework builds upon a whole-body MPC planner utilizing Sequential Linear Quadratic (SLQ) optimization. The core innovation is the integration of collision avoidance as soft constraints using Relaxed Barrier Functions (RBF). This formulation penalizes potential collisions in the cost function, allowing the solver to generate smooth, safe trajectories in real-time.
The system addresses two primary types of collisions:
- Self-Collision: The robot's geometry is modeled using primitive shapes (boxes and cylinders). The Gilbert-Johnson-Keerthi (GJK) algorithm is employed to calculate distances between these components. The authors found that using simplified robot models provided the best balance between safety and computational speed.
- Environment-Collision: Utilizing onboard LiDAR, the robot constructs a Euclidean Signed Distance Field (ESDF). To facilitate rapid queries, the robot's complex geometry is approximated as a set of spheres. The MPC then checks the distance from these sphere centers to the nearest obstacle in the ESDF map.
The entire control stack runs onboard the robot, with the MPC loop planning at 70 Hz and a low-level Whole-Body Controller (WBC) executing at 400 Hz.
Key Insights
- The "Tail" Effect: By unifying locomotion and manipulation, the MPC naturally utilizes the arm as a balancing appendage. When the robot's base tilts, the arm swings to counteract momentum, ensuring stability while simultaneously avoiding contact with the robot's torso.
- Efficiency of Naive Checks: Interestingly, standard "broad-phase" collision detection techniques often used in computer graphics were found to slow down the MPC. Hierarchical checks introduced discontinuities in the gradient, hindering solver convergence. A "naive" check of a simplified model proved more efficient for real-time control.
- Robustness of Soft Constraints: Using soft penalties rather than hard constraints prevents the solver from failing in tight spaces, making the system more robust for real-world deployment where sensor noise and minor model inaccuracies are present.
Experimental Results
Hardware validation was performed using an ANYmal C quadruped equipped with a 4-DoF DynaArm. The addition of self-collision avoidance increased MPC computation time by only 22-29%, while environment avoidance added approximately 12%, maintaining the high-frequency control necessary for stability.
The robot successfully performed dynamic maneuvers, including throwing a 1.5kg dumbbell. While a "blind" MPC resulted in the arm striking the robot's handle in simulation, the collision-aware MPC autonomously adjusted the arm's trajectory to clear the obstacle while hitting the target.
In navigation tests, the robot successfully traversed a doorway and avoided a moving cart. The MPC adapted the robot's torso orientation (yaw and roll) in real-time to maintain balance and clear the opening, demonstrating the effectiveness of the integrated ESDF mapping.
Conclusion and Future Work
While the framework demonstrates significant progress in unified control, limitations remain. LiDAR blind spots created by the arm's movement and sensor latency (15 Hz map updates) can affect reactions to fast-moving dynamic obstacles. Future research will focus on tighter integration of hard constraints and improved sensor fusion to eliminate blind spots and further enhance safety in highly dynamic environments.
Summary updated