Parallel Local Planning in Robotics

• Parallel local planning is a method that divides local trajectory computations across multiple threads to enhance real-time response and decision-making.
• It integrates advanced non-point-mass obstacle models—such as ellipsoids, polytopes, and composite shapes—to capture spatial complexities and ensure precise collision avoidance.
• By employing smooth nonlinear constraints and fast optimization techniques, parallel local planning achieves sub-50 ms iteration rates for navigating high-density, dynamic settings.

Non-point-mass obstacle modeling refers to the mathematical, computational, and algorithmic representation of obstacles as bodies with spatial extent and internal geometry, rather than as singular points. This paradigm enables accurate modeling of collision risk, clearance, and avoidance maneuvers in robotic, vehicular, communication, and flow environments where the “volume” or “shape” of obstacles fundamentally governs system dynamics and safety. Advanced methods capture full-body geometries through parametric sets such as ellipsoids, polytopes, superellipsoids, Gaussian process–encoded regions, or Bézier-defined boundaries, supporting both convex and complex nonconvex scenarios.

1. Mathematical Representations of Non-Point Obstacles

Standard non-point-mass obstacle models describe the forbidden subset of configuration space by explicit analytic conditions. The most prevalent forms include:

• Ellipsoidal obstacles: Defined in $\mathbb{R}^n$ by

$$\mathcal{E}(M,p) = \{\,x \in \mathbb{R}^n : (x - p)^\top M (x - p) \le 1\,\}$$

where $M \succ 0$ encodes axes and orientation (Rosenfelder et al., 16 Dec 2024, Lutz et al., 2021, Berkane et al., 2021, Kaymaz et al., 2022).

• Convex polytopes and semialgebraic sets: Represented as intersections of affine or nonlinear inequalities,

$$\mathcal{P} = \{\,z \mid A z \leq b\,\}, \qquad \mathcal{O} = \{\,z \mid g_i(z) \leq 0,\, \forall i\,\}$$

capturing more general convex and some nonconvex geometries (Lutz et al., 2021, Sathya et al., 2019, Helling et al., 2021).

• Superellipsoids and Superquadrics: Generalize ellipsoids using $p$-norms,

$$X = \{\,R S x + c \mid x \in B_p\,\},\quad B_p = \{\,x : \|x\|_p \le 1\,\}$$

and in implicit form (3D superquadrics),

$$C(x) = \left( \left( \frac{x_1}{a_1} \right)^{\varepsilon_2} + \left( \frac{x_2}{a_2} \right)^{\varepsilon_2} \right)^{\varepsilon_1/\varepsilon_2} + \left( \frac{x_3}{a_3} \right)^{\varepsilon_1} - 1$$

allowing interpolation between ellipsoids and boxes (Moran et al., 22 Apr 2024, Ginesi et al., 2020).

• Composite and free-form obstacles: Piecewise unions and intersections of primitives, fitted via variational GMMs or minimum-enclosing approaches, and parametrized by Bézier curves for optimization (Kaymaz et al., 2022, Cristiani et al., 2015).
• Probabilistic and fuzzy spatial models: Occupancy fields represented by stochastic processes (e.g., Gaussian Processes for potential/recognized regions (Zhang et al., 18 Oct 2025)) or level-set distance functions (e.g., $\Phi(x)=\mathrm{dist}(x,O)$ (Cristiani et al., 2015)).

Such parametrizations support smooth function evaluation, analytic differentiation, and embedding into trajectory optimization and control problems.

2. Computational Methods for Overlap and Separation

Collision or proximity predicates between non-point bodies require efficient tests. Key approaches include:

• Ellipsoid–ellipsoid overlap via $\lambda$-interpolation (Gilitschenski & Hanebeck):

For two ellipsoids with quadratic forms $A$, $B$ and centers $v$, $w$, define the interpolated ellipsoid

$$E_\lambda = \lambda A + (1-\lambda) B, \quad m_\lambda = E_\lambda^{-1}(\lambda A v + (1-\lambda) B w)$$

and the algebraic overlap metric

$$K(\lambda) = 1 - \lambda v^\top A v - (1-\lambda) w^\top B w + m_\lambda^\top E_\lambda m_\lambda$$

which reduces, in $\mathbb{R}^2$, to a cubic in $\lambda$. The minimum $f(\lambda^*)$ assesses separation: negative for disjoint, zero for tangent, positive for overlap. This method admits closed-form roots and $O(1)$ evaluation per test (Rosenfelder et al., 16 Dec 2024).

• Signed-distance and support-function methods:

For convex sets $V$, $\mathcal{O}$, the signed distance

$$d_\mathrm{signed}(V, \mathcal{O}) = \sup_{\|\hat{z}\|=1}\inf_{y\in V, x\in\mathcal{O}} \hat{z}^\top(y-x)$$

can be dualized into auxiliary variables (multipliers), yielding norm and consistency constraints suitable for embedding in an NLP. Separating hyperplanes for superellipsoids rely on support functions $\|S R^\top a\|_q + \langle a, c\rangle$ with the dual norm exponent $q$ (Helling et al., 2021, Moran et al., 22 Apr 2024).

• Constructive solid geometry and LogSumExp:

The union/intersection of primitive sets is encoded via smoothed maximum or minimum approximations (e.g., p-norm or LogSumExp), bounding true distances and providing differentiable constraints (Lutz et al., 2021).

• Level-set and hybrid flow/jump set approaches:

Level-set functions (e.g., $g(x)$, $d(x)$ for ellipsoids or opacity-aware sets $S^o(x)$ for pedestrian obstacles) describe internal, boundary, and exterior regions, supporting mode-dependent control logic (Berkane et al., 2021, Cristiani et al., 2015).

• Probabilistic collision inference:

For uncertain or partially observed obstacles, Gaussian process–modeled occupancy $P(x)$ allows for intersection probability calculation and integration into risk metrics, using convolution and recursive update (Zhang et al., 18 Oct 2025).

3. Embedding in Trajectory Optimization and Control

Non-point-mass obstacle models are embedded in model predictive control (MPC), optimal control problem (OCP) formulations, and motion planners as nonlinear equality or inequality constraints:

• Smooth nonlinear constraints:

Each predicted step imposes, for robot footprint $\mathcal{E}_r(x(k))$ and obstacle $\mathcal{E}_c$, a constraint such as $$f(\lambda^\star, \mathcal{E}_r(x(k)), \mathcal{E}_c) = \kappa^\star(x(k)) < 0$$, with $\kappa^\star$ the closed-form overlap metric (Rosenfelder et al., 16 Dec 2024). For superellipsoids, separation is imposed via

$$\|S^v R^v_{\theta} a\|_q + \|S^e R^e_{\theta} a\|_q + \langle a, c^v - c^e\rangle \leq 0, \quad \|a\|_2 = 1$$

at each stage (Moran et al., 22 Apr 2024).

• Penalty/soft-constraint methods:

Logical or “at least one” constraints arising from multiple inequalities per obstacle are recast as smooth penalty functions,

$$\psi_O(z) = \frac{1}{2} \prod_{i=1}^m [\max\{h^i(z),0\}]^2 = 0$$

and penalized in the OCP stage cost, preserving differentiability (Sathya et al., 2019).

• Dual multiplier and equality methods:

Dualization yields auxiliary variables enforcing separation between convex polytopes or system bodies, with auxiliary norm and consistency equalities (Helling et al., 2021).

• Sensing, risk, and probabilistic objectives:

In mixed-observed environments (e.g., UAV FOV-limited navigation), the occupancy or “threat” field $P(x)$ drives an urgency-weighted cost term integrated over configuration or control points, optimized for not just avoidance but active perception (Zhang et al., 18 Oct 2025).

Such encodings interface directly with nonlinear solvers (e.g., IPOPT, PANOC, OpEn), leveraging analytic gradients for real-time feasibility.

4. Applications Across Domains

Non-point-mass modeling is foundational in multiple domains:

• Mobile robotics: Full-volume collision avoidance in real-time MPC for differentially driven and omni-directional platforms, implicating both vehicle and obstacle body geometry (Rosenfelder et al., 16 Dec 2024, Moran et al., 22 Apr 2024).
• Maritime and aerial vehicles: Convex polygonal and ellipsoidal models for vessels in obstacle-dense waters, with explicit handling of dynamic and moving obstacles (Helling et al., 2021, Lutz et al., 2021, Kaymaz et al., 2022).
• Dense flows and crowds: Macroscopic pedestrian models exploit impermeable and opaque obstacles defined by Bézier or level-set boundaries, with PSO-optimized placement to exploit Braess’s paradox and minimize evacuation time (Cristiani et al., 2015).
• Communication networks: Path-loss estimation in NLoS UV links accounts for cuboid obstacles, integrating their full contour, position, and orientation into radiative transfer integrals, showing that accurate boundary modeling is essential for reflection-dominated regimes (Wu et al., 8 Nov 2024).
• Robot learning and DMPs: Volumetric, superquadric obstacles enable smooth, velocity-aware repulsive potentials yielding faithful collision avoidance in DMP-based motion generation across manipulators, surgical robots, and mobile bases (Ginesi et al., 2020).
• Fast point cloud–to–collision constraint pipelines: Ellipsoidal decomposition of point clouds via variational mixture modeling and minimum-volume covering enables real-time perception-to-planning algorithms (Kaymaz et al., 2022).

5. Comparative Properties and Computational Trade-offs

A variety of representations are compared in terms of fidelity, efficiency, and suitability for inclusion in real-time optimization:

Approach	Shape Fidelity	Dimensionality / Variables	Real-time Suitability
Ellipsoid	Poor for non-elliptic	Minimal (N_obs x N_pts)	Excellent
Signed-distance (GJK)	High (convex, polygon)	No extra variables	Excellent with compiled code
Polytope (dual)	High (convex, polygon)	Many multipliers per obstacle	Less suitable for high-dim
CSG + LogSumExp	Arbitrary (composite)	N_pts, no duals	Excellent, scalable
Superellipsoid	Smooth, parameteric	Low, analytic gradients	Efficient for convex shapes
GP-based (SPOT)	Probabilistic	Moderate (per GP update)	Sub-10 ms with sparse obs
Level-set/Bézier	Arbitrary (user-defined)	Grid-based, moderate	Efficient for 2D crowd models

Ellipsoidal constraints are computationally cheapest but can be overly conservative or imprecise for complex geometries. CSG/LogSumExp and signed-distance (GJK/EPA) offer high-fidelity and scalable implementations, with CSG especially suited to compositional or multi-body problems. Superellipsoids interpolate between ellipsoid and box representations, fitting applications where shape flexibility and analytic gradients are both needed. Level-set and GP-based models excel in distributed estimation and environments with uncertainty or occlusion.

6. Shape Optimization, Learning, and Geometry Identification

Non-point-mass modeling also underpins obstacle placement and morphology optimization:

• Shape optimization: Placement of symmetric obstacles inside confining domains for eigenvalue or evacuation minimization is framed as optimization over the obstacle’s orientation/position in the host geometry, with critical and extremal points determined analytically for dihedral-symmetric bodies (Chorwadwala et al., 2017).
• Data-driven geometry extraction: Real-time decomposition of sensor point clouds into minimum-volume ellipsoids or superquadrics, followed by tracking of features (center, axes, orientation) and kinematic estimation (velocity, angular rate), is achieved via variational inference, semi-definite programming, and greedy merging algorithms (Kaymaz et al., 2022).
• Probabilistic map fusion: Probabilistic belief maps fuse hypothesized and confirmed regions, permitting “obstacle-aware” sensing and planning without prior geometric knowledge (Zhang et al., 18 Oct 2025).

7. Synthesis and Outlook

Non-point-mass obstacle modeling enables precise, provably safe, and computationally tractable integration of obstacle geometry into control and planning loops. Methods range from analytical (ellipsoidal, polytope, superellipsoid) to compositionally defined (CSG, Bézier) to probabilistic or learning-based (GP, point-cloud decomposition). Efficient formulations for overlap, signed-distance, and support function evaluation are now standard in embedded real-time systems, and modern methods support hardware close-of-loop rates ($\lesssim$50 ms/iteration for multi-obstacle scenarios (Rosenfelder et al., 16 Dec 2024, Moran et al., 22 Apr 2024)).

Comparative studies demonstrate that while simple ellipsoidal proxies incur conservatism, their O(1) cost and smooth, differentiable constraints make them attractive for embedded platforms. Advanced schemes leveraging composite primitives or probabilistic fields enable adaptation to arbitrary shapes and dynamic, uncertain environments at the cost of modestly increased complexity. Ongoing research aims to further reduce computational cost, generalize to nonconvex and articulated obstacles, improve uncertainty modeling, and automate geometry discovery from high-throughput sensor streams.

Non-point-mass modeling forms the foundational substrate of modern robotic and vehicular autonomy, crowd simulation, sensor-aware planning, and communication system design, enabling robust performance in realistic, cluttered, and dynamic environments.