Rigid Body Path Planning using Mixed-Integer Linear Programming

TL;DR

This work tackles rigid-body motion planning in cluttered environments where narrow passages hinder traditional planners. It introduces a three-stage MILP-based pipeline that offline-constructs a reusable graph of convex polytopes $G_c$ to cover the free workspace, offline-builds a dense graph $G_d$ over boundary intersections, and online connects start/end configurations to retrieve a feasible path using Dijkstra while performing MILP-based verifications of path segments. The key contributions are (1) workspace-based decomposition that mitigates narrow-tunnel challenges, (2) splitting the large optimization into small, local MILPs, and (3) extensive 2D/3D experiments showing shorter online times and improved scalability compared to PRM, WCO, and GCS. This approach enables fast, multi-query planning for complex rigid-body geometries in crowded spaces with exact collision checking, offering practical benefits for real-time planning in robotics.

Abstract

Navigating rigid body objects through crowded environments can be challenging, especially when narrow passages are presented. Existing sampling-based planners and optimization-based methods like mixed integer linear programming (MILP) formulations, suffer from limited scalability with respect to either the size of the workspace or the number of obstacles. In order to address the scalability issue, we propose a three-stage algorithm that first generates a graph of convex polytopes in the workspace free of collision, then poses a large set of small MILPs to generate viable paths between polytopes, and finally queries a pair of start and end configurations for a feasible path online. The graph of convex polytopes serves as a decomposition of the free workspace and the number of decision variables in each MILP is limited by restricting the subproblem within two or three free polytopes rather than the entire free region. Our simulation results demonstrate shorter online computation time compared to baseline methods and scales better with the size of the environment and tunnel width than sampling-based planners in both 2D and 3D environments.

Figures (10)

• Figure 1: The figure demonstrates an example solved with our MILP-based method, navigating a non-convex, V-shaped object through a complex environment from the start configuration to the goal configuration, both marked red. In the figure, the obstacles are shown in pink, while the green depicts the regions swept by the object, demonstrating the effectiveness and precision of our approach in challenging environments.
• Figure 2: An overview of our pipeline. (a) Workspace decomposition: Decomposition of the free workspace into a graph of convex polytopes $G_c$. The polytopes serve as graph vertices and are interconnected if they overlap (\ref{['sec:coarse_graph']}). (b-d) Path segment validation: Construction of $G_d$(\ref{['sec:dense_graph']}), where each vertex is a set of interconnected free configurations - illustrated as the set of green points enclosed by the green ring. The motions between vertices (blue edges) are generated by solving MILPs (\ref{['sec:dense-node-full']}). (e) Online query: the start and end configurations are connected to graph $G_d$ and the planned path is retrieved (\ref{['sec:inference']}).
• Figure 3: The figure shows the reachable set of 2D objects for translation and rotation scenarios, respectively. On the right, we illustrate the approximation of the green sectors to yellow polytopes.
• Figure 4: Models of collision avoidance around obstacles (gray). The free space is decomposed into convex polytopes (blue and yellow). Green lines indicate predicted collision-free paths and red lines indicate detected collisions. Left: Less strict encoding that only checks endpoints of subdivided line segments. The dashed red line demonstrates that endpoint-only checking is insufficient as it cuts through an obstacle. Right8613017: This overly restrictive model allows only paths like the dotted line where both endpoints lie within the same free region, rejecting the simpler, viable solid line. Middle (ours): Strict encoding based on \ref{['thm:line']}. This approach prevents the dashed line from cutting through obstacles by incorporating additional constraints beyond endpoint checking, while including more paths between regions, like the solid line.
• Figure 5: Visualization of planning results in 2D (obstacles: pink) and 3D scenarios. The start and end configurations are plotted with red, while waypoints and their corresponding reachable sets are shown in green. In 2D scenarios, we also illustrate the graph $G_c$ with vertices ($\mathcal{V}_c$) as blue polytopes and edges ($\mathcal{E}_c$) as blue lines connecting red dots, which represent the centers of vertex polytopes. \ref{['fig:2d-peg-in-hole', 'fig:3d-peg-in-hole']} show the planning results of L-shape non-convex objects, while the rest show the results of convex objects.

Theorems & Definitions (6)

• Proposition 1: Line segment inside two convex polytopes
• proof
• Corollary 1.1: Triangle
• proof
• Corollary 1.2: Convex quadrilateral
• proof