Stochastic Model Predictive Control with Optimal Linear Feedback for Mobile Robots in Dynamic Environments

TL;DR

The paper tackles robot motion planning in dynamic environments where human motion is uncertain. It introduces a stochastic MPC framework that optimizes a time-varying linear feedback $\tilde{u}_k^{\mathrm{r}}(\tilde{x}_k) = u_k^{\mathrm{r}} + K_k(\tilde{x}_k - x_k)$ to mitigate growing human-motion uncertainty and improve reference-tracking while probabilistically enforcing collision avoidance. Key components include covariance propagation of the joint robot-human state, an expected-cost formulation that accounts for uncertainty, and penalized chance constraints to maintain feasibility. Through comparisons of full-state, partial-state, and no-feedback variants, the results show improved performance with manageable computation time, with partial-state feedback offering a favorable trade-off between cost and speed. The approach enhances passive safety by balancing uncertainty reduction with state disturbance, enabling safer navigation in dynamic human-robot environments.

Abstract

Robot navigation around humans can be a challenging problem since human movements are hard to predict. Stochastic model predictive control (MPC) can account for such uncertainties and approximately bound the probability of a collision to take place. In this paper, to counteract the rapidly growing human motion uncertainty over time, we incorporate state feedback in the stochastic MPC. This allows the robot to more closely track reference trajectories. To this end the feedback policy is left as a degree of freedom in the optimal control problem. The stochastic MPC with feedback is validated in simulation experiments and is compared against nominal MPC and stochastic MPC without feedback. The added computation time can be limited by reducing the number of additional variables for the feedback law with a small compromise in control performance.

Figures (5)

• Figure 1: Planned trajectories. The solid blue lines and solid orange lines in the upper plots represent the nominal trajectory of the robot and human respectively. The blue circles are of radius $\Delta_{\text{safe}}$, and they correspond to time instant 0.0 s, 0.5 s, 1.0 s, 1.5 s, and 2.0 s respectively. Each blue semi-transparent set depicts the Minkowski sum of a radius-$\Delta_{\text{safe}}$ circle and the ellipsoid corresponding to the three standard deviations of the robot position uncertainty. The orange ellipsoids and the shadows in the distance plots represent uncertainty tubes (three standard deviations).
• Figure 2: Planned robot state trajectories in time. Full-state feedback is incorporated. The shadows represent uncertainty tubes (three standard deviations). The red ellipsoid is for highlighting purpose.
• Figure 3: Robot and human trajectories in an MPC simulation of five seconds. The blue markers depict the robot positions at certain time instants. The orange semi-transparent sets correspond to the modeled human occupancy in space (three standard deviations).
• Figure 4: Mean stage cost and minimum robot-human distance. Purple triangles and green circles correspond to $\gamma=3.0$ and $\gamma=2.0$.
• Figure 5: Computation time of solving OCPs ($\gamma=3.0$). The box extends from the lower to upper quartile of the data. The dashed line shows the median.