arXiv subjects

Hongzhe Yu

Publications and source records attributed to Hongzhe Yu.

14 recordsLinked to original sources

arXiv2026

Function-Space Diffusion for Motion Planning

Diffusion-based motion planners have demonstrated strong performance in generating diverse and high-quality robot trajectories in cluttered environments with multiple feasible solutions. However, existing approaches typically operate on fixed-length waypoint sequences, making the learned model resolution-dependent, thereby preventing zero-shot generalization across resolutions. In this work, we propose Function-Space Diffusion for Motion Planning (FSD-MP), a diffusion-based motion planner that models trajectories as continuous functions and performs diffusion directly in function space, achieving discretization-invariant trajectory generation. We define a mode-wise forward process in the spectral domain, driven by Gaussian noise with a Mat\'ern-type covariance, and parameterize the reverse process with a boundary-compatible Discrete Sine Transform-based Fourier Neural Operator (DST-FNO) that preserves start-goal constraints across resolutions. We evaluate FSD-MP on 2D point robot and 7-DoF Franka manipulator planning benchmarks. Our method achieves competitive planning performance at the training resolution and generalizes zero-shot across resolutions up to 16$\times$ higher, preserving consistent planning behavior without retraining. These results demonstrate that function-space diffusion provides an effective framework for discretization-invariant motion planning.

cs.RO↗

arXiv2026

PISTO: Proximal Inference for Stochastic Trajectory Optimization

Stochastic trajectory optimization methods like STOMP enable planning with non-differentiable costs, offering substantial flexibility over gradient-based approaches. We show that STOMP implicitly minimizes the KL divergence from a Boltzmann trajectory distribution, revealing an elegant Variational Inference (VI) structure underlying its updates. Building on this insight, we propose the \textit{Proximal Inference for Stochastic Trajectory Optimization} (PISTO) algorithm that stabilizes the updates by augmenting the objective with a KL regularization between successive Gaussian proposals. This proximal formulation admits a trust-region interpretation and yields closed-form mean updates computable as expectations under a surrogate distribution. We estimate these expectations via importance-weighted Monte Carlo sampling, producing a simple, derivative-free algorithm that inherits STOMP's ability to handle non-differentiable and discontinuous costs without modification. On robot arm motion planning benchmarks, PISTO achieves an 89\% success rate -- outperforming CHOMP (63\%) and STOMP (68\%) -- while producing shorter, smoother paths at twice the speed of competing stochastic methods. We further validate PISTO on contact-rich MuJoCo locomotion and manipulation tasks, where it consistently outperforms both CEM and MPPI baselines in reward.

cs.RO↗

arXiv2026

Concentration of Stochastic System Trajectories with Time-varying Contraction Conditions

We establish two concentration inequalities for nonlinear stochastic system under time-varying contraction conditions. The key to our approach is an energy function termed Averaged Moment Generating Function (AMGF). By combining it with incremental stability analysis, we develop a concentration inequality that bounds the deviation between the stochastic system state and its deterministic counterpart. As this inequality is restricted to single time instance, we further combine AMGF with martingale-based methods to derive a concentration inequality that bounds the fluctuation of the entire stochastic trajectory. Additionally, by synthesizing the two results, we significantly improve the trajectory-level concentration inequality for strongly contractive systems. Given the probability level $1-\delta$, the derived inequalities ensure an $\mO(\sqrt{\log(1/\delta))}$ bound on the deviation of stochastic trajectories, which is tight under our assumptions. Our results are exemplified through a case study on stochastic safe control.

math.OC↗

arXiv2026

Path Integral Particle Filtering for Hybrid Systems via Saltation Matrices

State estimation for hybrid systems that undergo intermittent contact with their environments, such as extraplanetary robots and satellites undergoing docking operations, is difficult due to the discrete uncertainty propagation during contact. To handle this propagation, this paper presents an optimal-control-based particle filtering method that leverages saltation matrices to map out uncertainty propagation during contact events. By exploiting a path integral filtering framework that exploits the duality between smoothing and optimal control, the resulting state estimation algorithm is robust to outlier effects, flexible to non-Gaussian noise distributions, and handles challenging contact dynamics in hybrid systems. To evaluate the validity and consistency of the proposed approach, this paper tests it against strong baselines on the stochastic dynamics generated by a bouncing ball and spring loaded inverted pendulum.

cs.RO↗

arXiv2025

Safety Verification of Stochastic Systems under Signal Temporal Logic Specifications

We study the verification problem of stochastic systems under signal temporal logic (STL) specifications. We propose a novel approach that enables the verification of the probabilistic satisfaction of STL specifications for nonlinear systems subject to both bounded deterministic disturbances and stochastic disturbances. Our method, referred to as the STL erosion strategy, reduces the probabilistic verification problem into a deterministic verification problem with a tighter STL specification. The degree of tightening is determined by leveraging recent results on bounding the deviation between the stochastic trajectory and the deterministic trajectory. Our approach can be seamlessly integrated with any existing deterministic STL verification algorithm. Numerical experiments are conducted to showcase the efficacy of our method.

cs.LO↗

arXiv2024

Efficient Iterative Proximal Variational Inference Motion Planning

We cast motion planning under uncertainty as a stochastic optimal control problem, where the optimal posterior distribution has an explicit form. To approximate this posterior, this work frames an optimization problem in the space of Gaussian distributions by solving a Variational Inference (VI) in the path distribution space. For linear-Gaussian stochastic dynamics, a proximal algorithm is proposed to solve for an optimal Gaussian proposal iteratively. The computational bottleneck is evaluating the gradients with respect to the proposal over a dense trajectory. To tackle this issue, the sparse planning factor graph and Gaussian Belief Propagation (GBP) are exploited, allowing for parallel computation of these gradients on Graphics Processing Units (GPUs). We term the novel paradigm the \textit{Parallel Gaussian Variational Inference Motion Planning (P-GVIMP)}. Building on the efficient algorithm for linear Gaussian systems, we then propose an iterative paradigm based on Statistical Linear Regression (SLR) techniques to solve planning problems for nonlinear stochastic systems, where the P-GVIMP serves as a sub-routine for the linearized time-varying system at each iteration. The proposed framework is validated on various robotic systems, demonstrating significant speed acceleration achieved by leveraging parallel computation and successful planning solutions for nonlinear systems under uncertainty. An open-sourced implementation is presented at \href{https://github.com/hzyu17/VIMP}{https://github.com/hzyu17/VIMP}.

cs.RO↗

arXiv2024

Path Integral Control for Hybrid Dynamical Systems

This work introduces a novel paradigm for solving optimal control problems for hybrid dynamical systems under uncertainties. Robotic systems having contact with the environment can be modeled as hybrid systems. Controller design for hybrid systems under disturbances is complicated by the discontinuous jump dynamics, mode changes with inconsistent state dimensions, and variations in jumping timing and states caused by noise. We formulate this problem into a stochastic control problem with hybrid transition constraints and propose the Hybrid Path Integral (H-PI) framework to obtain the optimal controller. Despite random mode changes across stochastic path samples, we show that the ratio between hybrid path distributions with varying drift terms remains analogous to the smooth path distributions. We then show that the optimal controller can be obtained by evaluating a path integral with hybrid constraints. Importance sampling for path distributions with hybrid dynamics constraints is introduced to reduce the variance of the path integral evaluation, where we leverage the recently developed Hybrid iterative-Linear-Quadratic-Regulator (H-iLQR) controller to induce a hybrid path distribution proposal with low variance. The proposed method is validated through numerical experiments on various hybrid systems and extensive ablation studies. All the sampling processes are conducted in parallel on a Graphics Processing Unit (GPU).

cs.RO↗

arXiv2024

Optimal Covariance Steering of Linear Stochastic Systems with Hybrid Transitions

This work addresses the problem of optimally steering the state covariance of a linear stochastic system from an initial to a target, subject to hybrid transitions. The nonlinear and discontinuous jump dynamics complicate the control design for hybrid systems. Under uncertainties, stochastic jump timing and state variations further intensify this challenge. This work aims to regulate the hybrid system's state trajectory to stay close to a nominal deterministic one, despite uncertainties and noises. We address this problem by directly controlling state covariances around a mean trajectory, and this problem is termed the Hybrid Covariance Steering (H-CS) problem. The jump dynamics are approximated to the first order by leveraging the Saltation Matrix. When the jump dynamics are nonsingular, we derive an analytical closed-form solution to the H-CS problem. For general jump dynamics with possible singularity and changes in the state dimensions, we reformulate the problem into a convex optimization over path distributions by leveraging Schrodinger's Bridge duality to the smooth covariance control problem. The covariance propagation at hybrid events is enforced as equality constraints to handle singularity issues. The proposed convex framework scales linearly with the number of jump events, ensuring efficient, optimal solutions. This work thus provides a computationally efficient solution to the general H-CS problem. Numerical experiments are conducted to validate the proposed method.

math.OC↗

arXiv2023

Efficient Belief Road Map for Planning Under Uncertainty

Robotic systems, particularly in demanding environments like narrow corridors or disaster zones, often grapple with imperfect state estimation. Addressing this challenge requires a trajectory plan that not only navigates these restrictive spaces but also manages the inherent uncertainty of the system. We present a novel approach for graph-based belief space planning via the use of an efficient covariance control algorithm. By adaptively steering state statistics via output state feedback, we efficiently craft a belief roadmap characterized by nodes with controlled uncertainty and edges representing collision-free mean trajectories. The roadmap's structured design then paves the way for precise path searches that balance control costs and uncertainty considerations. Our numerical experiments affirm the efficacy and advantage of our method in different motion planning tasks. Our open-source implementation can be found at https://github.com/hzyu17/VIMP/tree/BRM.

cs.RO↗

arXiv2023

Stochastic Motion Planning as Gaussian Variational Inference: Theory and Algorithms

We present a novel formulation for motion planning under uncertainties based on variational inference where the optimal motion plan is modeled as a posterior distribution. We propose a Gaussian variational inference-based framework, termed Gaussian Variational Inference Motion Planning (GVI-MP), to approximate this posterior by a Gaussian distribution over the trajectories. We show that the GVI-MP framework is dual to a special class of stochastic control problems and brings robustness into the decision-making in motion planning. We develop two algorithms to numerically solve this variational inference and the equivalent control formulations for motion planning. The first algorithm uses a natural gradient paradigm to iteratively update a Gaussian proposal distribution on the sparse motion planning factor graph. We propose a second algorithm, the Proximal Covariance Steering Motion Planner (PCS-MP), to solve the same inference problem in its stochastic control form with an additional terminal constraint. We leverage a proximal gradient paradigm where, at each iteration, we quadratically approximate nonlinear state costs and solve a linear covariance steering problem in closed form. The efficacy of the proposed algorithms is demonstrated through extensive experiments on various robot models. An implementation is provided in https://github.com/hzyu17/VIMP.

cs.RO↗

arXiv2023

Data-driven optimal control under safety constraints using sparse Koopman approximation

In this work we approach the dual optimal reach-safe control problem using sparse approximations of Koopman operator. Matrix approximation of Koopman operator needs to solve a least-squares (LS) problem in the lifted function space, which is computationally intractable for fine discretizations and high dimensions. The state transitional physical meaning of the Koopman operator leads to a sparse LS problem in this space. Leveraging this sparsity, we propose an efficient method to solve the sparse LS problem where we reduce the problem dimension dramatically by formulating the problem using only the non-zero elements in the approximation matrix with known sparsity pattern. The obtained matrix approximation of the operators is then used in a dual optimal reach-safe problem formulation where a linear program with sparse linear constraints naturally appears. We validate our proposed method on various dynamical systems and show that the computation time for operator approximation is greatly reduced with high precision in the solutions.

math.OC↗

arXiv2022

A Gaussian variational inference approach to motion planning

We propose a Gaussian variational inference framework for the motion planning problem. In this framework, motion planning is formulated as an optimization over the distribution of the trajectories to approximate the desired trajectory distribution by a tractable Gaussian distribution. Equivalently, the proposed framework can be viewed as a standard motion planning with an entropy regularization. Thus, the solution obtained is a transition from an optimal deterministic solution to a stochastic one, and the proposed framework can recover the deterministic solution by controlling the level of stochasticity. To solve this optimization, we adopt the natural gradient descent scheme. The sparsity structure of the proposed formulation induced by factorized objective functions is further leveraged to improve the scalability of the algorithm. We evaluate our method on several robot systems in simulated environments, and show that it achieves collision avoidance with smooth trajectories, and meanwhile brings robustness to the deterministic baseline results, especially in challenging environments and tasks.

cs.RO↗

arXiv2021

Covariance Steering for Nonlinear Control-affine Systems

We consider the covariance steering problem for nonlinear control-affine systems. Our objective is to find an optimal control strategy to steer the state of a system from an initial distribution to a target one whose mean and covariance are given. Due to the nonlinearity, the existing techniques for linear covariance steering problems are not directly applicable. By leveraging the celebrated Girsanov theorem, we formulate the problem into an optimization over the space path distributions. We then adopt a generalized proximal gradient algorithm to solve this optimization, where each update requires solving a linear covariance steering problem. Our algorithm is guaranteed to converge to a local optimal solution with a sublinear rate. In addition, each iteration of the algorithm can be achieved in closed form, and thus the computational complexity of it is insensitive to the resolution of time-discretization. In the examples, our method achieves 1000 times speedup over an existing algorithm.

math.OC↗

arXiv2019

Learning to Navigate from Simulation via Spatial and Semantic Information Synthesis with Noise Model Embedding

While training an end-to-end navigation network in the real world is usually of high cost, simulation provides a safe and cheap environment in this training stage. However, training neural network models in simulation brings up the problem of how to effectively transfer the model from simulation to the real world (sim-to-real). In this work, we regard the environment representation as a crucial element in this transfer process and propose a visual information pyramid (VIP) model to systematically investigate a practical environment representation. A novel representation composed of spatial and semantic information synthesis is then established accordingly, where noise model embedding is particularly considered. To explore the effectiveness of this representation, we compared the performance with representations popularly used in the literature in both simulated and real-world scenarios. Results suggest that our environment representation stands out. Furthermore, an analysis on the feature map is implemented to investigate the effectiveness through inner reaction, which could be irradiative for future researches on end-to-end navigation.

cs.RO↗