Non-Gaussian Risk Bounded Trajectory Optimization for Stochastic Nonlinear Systems in Uncertain Environments

Abstract

We consider the motion planning problem for stochastic nonlinear systems in uncertain environments. More precisely, in this problem the robot has stochastic nonlinear dynamics and uncertain initial locations, and the environment contains multiple dynamic uncertain obstacles. Obstacles can be of arbitrary shape, can deform, and can move. All uncertainties do not necessarily have Gaussian distribution. This general setting has been considered and solved in [1]. In addition to the assumptions above, in this paper, we consider long-term tasks, where the planning method in [1] would fail, as the uncertainty of the system states grows too large over a long time horizon. Unlike [1], we present a real-time online motion planning algorithm. We build discrete-time motion primitives and their corresponding continuous-time tubes offline, so that almost all system states of each motion primitive are guaranteed to stay inside the corresponding tube. We convert probabilistic safety constraints into a set of deterministic constraints called risk contours. During online execution, we verify the safety of the tubes against deterministic risk contours using sum-ofsquares (SOS) programming. The provided SOS-based method verifies the safety of the tube in the presence of uncertain obstacles without the need for uncertainty samples and time discretization in real-time. By bounding the probability the system states staying inside the tube and bounding the probability of the tube colliding with obstacles, our approach guarantees bounded probability of system states colliding with obstacles. We demonstrate our approach on several long-term robotics tasks.

Convex risk-bounded continuous-time trajectory planning and tube design in uncertain nonconvex environments

“…Our algorithms do not consider system dynamics. For the planner that considers stochastic nonlinear dynamics, refer to Han et al (2022).…”

Lie Algebraic Cost Function Design for Control on Lie Groups

“…From the point of view of computational complexity, the worst-case for arbitrary obstacle configuration still results in SDPs of computationally intractable size. Thus, a convex representation of free space [15], obstacles [30,67,68], or risk-bounded contours of [69][70][71] should be explored.…”

Moment-based Kalman Filter: Nonlinear Kalman Filtering with Exact Moment Propagation

“…where u k is an input vector at time step k. In this paper, we assume dynamic and measurement models are described by mixed-trigonometric-polynomial functions. This is not a conservative assumption because these elementary functions can represent a wide range of robotic system models [26]. Note that external disturbance w k and measurement noise v k are independent of x k and y k .…”

“…Unlike conventional approaches, the proposed Moment-based Kalman Filter (MKF) does not use system approximation or sampling methods for moment propagation. Instead, MKF propagates exact moments of state distributions by extending the recently proposed method [24]- [26], which computes exact mixed-trigonometric-polynomial moments of uncertain states up to any desired order. The extended method of this paper can also compute exact mixed-trigonometricpolynomial moments of non-independent Gaussian random variables.…”