[Paper Review] Toward Safety-Aware Informative Motion Planning for Legged Robots

Sangli Teng, Yukai Gong+2 more|arXiv (Cornell University)|Mar 26, 2021

Robotic Path Planning Algorithms52 references22 citations

TL;DR

This paper proposes SAFE-IIG, a safety-aware, informative motion planning framework for legged robots that integrates discrete-time Control Barrier Functions (DCBF) with Model Predictive Control (MPC) for multi-step, dynamically feasible trajectory generation. By combining DCBF-MPC with the Incrementally-exploring Information Gathering (IIG) algorithm, the planner ensures safety and stability while maximizing information gain from a stochastic map, achieving collision-free, dynamically feasible paths in simulation without replanning.

ABSTRACT

This paper reports on developing an integrated framework for safety-aware informative motion planning suitable for legged robots. The information-gathering planner takes a dense stochastic map of the environment into account, while safety constraints are enforced via Control Barrier Functions (CBFs). The planner is based on the Incrementally-exploring Information Gathering (IIG) algorithm and allows closed-loop kinodynamic node expansion using a Model Predictive Control (MPC) formalism. Robotic exploration and information gathering problems are inherently path-dependent problems. That is, the information collected along a path depends on the state and observation history. As such, motion planning solely based on a modular cost does not lead to suitable plans for exploration. We propose SAFE-IIG, an integrated informative motion planning algorithm that takes into account: 1) a robot's perceptual field of view via a submodular information function computed over a stochastic map of the environment, 2) a robot's dynamics and safety constraints via discrete-time CBFs and MPC for closed-loop multi-horizon node expansions, and 3) an automatic stopping criterion via setting an information-theoretic planning horizon. The simulation results show that SAFE-IIG can plan a safe and dynamically feasible path while exploring a dense map.

Motivation & Objective

• To address the lack of safety and dynamic feasibility in existing sampling-based informative motion planning for legged robots.
• To integrate legged robot dynamics and safety constraints into information-gathering planning via control barrier functions and model predictive control.
• To develop a receding-horizon, multi-step planner that enables closed-loop node expansion while maintaining safety and information maximization.
• To enable automatic stopping based on information-theoretic criteria, avoiding unnecessary exploration.
• To validate the feasibility and robustness of planned trajectories through high-fidelity simulation of a Cassie-series biped robot.

Proposed method

• Uses a discrete-time Control Barrier Function (DCBF) to enforce forward invariance of a safe set, ensuring collision avoidance and stability.
• Employs Model Predictive Control (MPC) in a receding horizon fashion to generate multi-step, dynamically feasible trajectories for node expansion.
• Integrates the DCBF-MPC planner into the Incrementally-exploring Information Gathering (IIG) framework to enable safety-critical, information-maximizing path planning.
• Computes information gain using a submodular function over a dense, stochastic map of the environment to prioritize informative paths.
• Applies a planning horizon based on information-theoretic stopping criteria to terminate exploration when further gains are negligible.
• Uses a simplified LIP model with angular momentum feedback for low-level control, enabling closed-loop tracking of planned waypoints and velocities.

Experimental results

Research questions

• RQ1 How can safety and dynamic feasibility be formally encoded in sampling-based informative motion planning for legged robots?
• RQ2 Can a multi-step, closed-loop trajectory planner based on DCBF-MPC improve path quality and safety compared to one-step greedy methods?
• RQ3 How can information gain from a stochastic map be effectively integrated into a safety-aware, kinodynamically feasible planning framework?
• RQ4 Can the planner automatically determine when to stop exploration based on information-theoretic criteria without prior knowledge of the environment?
• RQ5 To what extent can the planned trajectories be tracked in closed-loop by a legged robot controller without replanning?

Key findings

• SAFE-IIG successfully plans collision-free, dynamically feasible paths in complex environments using a Cassie-series biped robot model.
• Closed-loop path tracking error for CoM position remains below 0.05 m, demonstrating high tracking accuracy without replanning.
• Open-loop tracking drift is limited to less than 0.8 m over 124 steps in a single-obstacle environment and less than 0.4 m in a cave map with 115 steps.
• The planner achieves smoother trajectories compared to baseline RRT-DCBF-MPC, with reduced path deviation and improved stability.
• The integration of DCBF-MPC enables safe, multi-step node expansion, ensuring that each new node is dynamically feasible and collision-free.
• The information-theoretic stopping criterion effectively terminates exploration when further gains are minimal, improving planning efficiency.