Organizations: Dept. of Aerospace Engineering, University of Maryland, College Park, MD, USA. · Computer, Electrical and Mathematical Science & Engineering Division (CEMSE), KAUST, Saudi Arabia. · Dept. Electrical and Computer Engineering and Dept. Computer Science, University of Maryland, College Park, MD, USA.
Autonomous navigation in complex, non-convex environments remains challenging when robot dynamics, control limits, and exact robot geometry must all be taken into account. In this paper, we propose a hierarchical planning and control framework that bridges long-horizon guidance and geometry-aware safety guarantees for a polytopic robot navigating among polytopic obstacles. At the high level, Mixed-Integer Linear Programming (MILP) is embedded within a Model Predictive Control (MPC) framework to generate a nominal trajectory around polytopic obstacles while modeling the robot as a point mass for computational tractability. At the low level, we employ a control barrier function (CBF) based on the exact signed distance in the Minkowski-difference space as a safety filter to explicitly enforce the geometric constraints of the robot shape, and further extend its formulation to a high-order CBF (HOCBF). We demonstrate the proposed framework in U-shaped and maze-like environments under single- and double-integrator dynamics. The results show that the proposed architecture mitigates the topology-induced local-minimum behavior of purely reactive CBF-based navigation while enabling safe, real-time, geometry-aware navigation.
Figures & tables
Figure 1: Top: The proposed multi-rate framework with a MILP-MPC planner and a geometry-aware Minkowski-CBF safety filter. Bottom: A triangular robot with double-integrator dynamics navigating in a polytopic maze-like environment. It collides with an obstacle when using MILP-MPC alone with a point-mass model (pink), and becomes trapped in a local minimum using a CLF-CBF-QP approach (orange) due to its inherent reactive nature. In contrast, the proposed framework (blue) successfully guides the robot to the goal without a reference path or waypoints from an external planner .
Figure 2: Long-horizon nominal predictions (dashed cyan) generated by the high-level MILP-MPC planner under (a) single- ( Top ) and (b) double-integrator ( Bottom ) dynamics, navigating the robot to the goal. Left: The actual closed-loop trajectory (solid blue) tracks the MPC predicted states (brown squares) as the robot (blue) avoids obstacles (gray). Right: Control inputs u and CBF values, verifying that strict safety h(x)≥0 is guaranteed with respect to the real robot’s geometry over time.
Figure 3: Top-row: Comparison of the proposed MILP-MPC-CBF framework in a U-shaped environment under single- ( Left column) and double-integrator ( Right column) dynamics. In each case, the left compares the proposed framework with MILP-MPC only, where the green segment indicates the interval during which the geometry-aware Minkowski-(HO)CBF safety filter is active, while the right compares it with the reactive CLF-(HO)CBF-QP. Bottom-row: Time histories of control inputs u and CBFs h for MILP-MPC-CBF, showing that the full-geometry robot remains in the safe set throughout the navigation.
Obstacle avoidance of polytopic obstacles by polytopic robots is a challenging problem in optimization-based control and trajectory planning. Many existing methods rely on smooth geometric approximations, such as hyperspheres or ellipsoids, which allow differentiable distance expressions but distort the true geometry and restrict the feasible set. Other approaches integrate exact polytope distances into nonlinear model predictive control (MPC), resulting in nonconvex programs that limit real-time performance. In this paper, we construct linear discrete-time control barrier function (DCBF) constraints by deriving supporting hyperplanes from exact closest-point computations between convex polytopes. We then propose a novel iterative convex MPC-DCBF framework, where local linearization of system dynamics and robot geometry ensures convexity of the finite-horizon optimization at each iteration. The resulting formulation reduces computational complexity and enables fast online implementation for safety-critical control and trajectory planning of general nonlinear dynamics. The framework extends to multi-robot and three-dimensional environments. Numerical experiments demonstrate collision-free navigation in cluttered maze scenarios with millisecond-level solve times.
Shuo Liu, Zhe Huang, Calin A. Belta
Boston University, Brookline, MA, USA · Department of Electrical and Computer Engineering and with the Department of Computer Science, University of Maryland, College Park, MD, USA
Autonomous mobile robots operating in tight environments require motion planning frameworks that account for the physical footprint of the robot. Simplifying the geometry to a point or a circle is conservative and discards information needed to successfully and safely traverse narrow passages. This work proposes a safe local motion planning and control method that guarantees that a polytopic robot footprint stays inside a continuously updated convex free-space region. The containment condition is formulated as a set of discrete-time control barrier function constraints within a model predictive controller. The number of safety constraints depends on the complexity of the local free-space geometry and the robot shape, instead of the number of obstacles. The proposed free-space formulation does not need any obstacle detection or segmentation. A comparative analysis against a polytope-based obstacle avoidance formulation confirms favorable scaling up to a reduction of 91× in computation time as the number of obstacles increases. The approach is validated in simulation with an autonomous surface vehicle and on hardware with a non-holonomic mobile robot, using both occupancy grids and LiDAR sensing. The experiments demonstrate safe real-time motion planning and control at 10~Hz on an onboard embedded computer, including reactive avoidance of dynamic obstacles.
Alejandro Gonzalez-Garcia, Dries Dirckx, Jan Swevers +1
Safely navigating polytopic environments while respecting the dynamics, control, and exact geometry of the underlying system is a challenge in robotics. Control barrier functions (CBFs) synthesize safe control policies by rendering the safe set forward invariant, but many existing CBF-based methods approximate polytopes using conservative smooth shapes, such as spheres or ellipsoids, to obtain explicit differentiable distance functions. In this article, we propose an exact Signed Distance Function (SDF) formulation for a {\it polytopic} robot and {\it polytopic} obstacles and integrate it with nonsmooth CBFs. Leveraging Minkowski operations, the proposed method computes the exact SDF via companion convex programs in both the collision-free (positive-sign) and in-collision (negative-sign) cases. Furthermore, by exploiting the convenient geometric properties of 2D Minkowski operations and the optimality conditions of the two companion convex programs, we derive a unified analytical expression for the gradient of the exact SDF via sensitivity analysis. The exact rotational gradient further reveals a previously masked class of local minima induced by the coupling between geometry and nonholonomic kinematics. We demonstrate the effectiveness of the proposed framework through a pure-translation case and three scenarios with unicycle models involving recovery from an unsafe initialization and single- and multiple-obstacle avoidance. Comparisons with baseline methods highlight how the proposed framework enables non-conservative maneuvers and safety recovery.
Yi-Hsuan Chen, Shuo Liu, Wei Xiao +2
Department of Aerospace Engineering, University of Maryland, College Park, MD 20742, USA · Department of Mechanical Engineering, Boston University, Boston, MA 02215, USA · School of Electrical and Electronic Engineering, Nanyang Technological University, and with M3S, SMART, Singapore +1