Probabilistic motion planning and optimization incorporating chance constraints

Abstract

For high-dimensional robots, motion planning is still a challenging problem, especially for manipulators mounted to underwater vehicles or human support robots where uncertainties and risks of plan failure can have severe impact. However, existing risk-aware planners mostly focus on low-dimensional planning tasks, meanwhile planners that can account for uncertainties and react fast in high degree-of-freedom (DOF) robot planning tasks are lacking. In this thesis, a risk-aware motion planning and execution system called Probabilistic Chekov (p-Chekov) is introduced, which includes a deterministic stage and a risk-aware stage. A systematic set of experiments on existing motion planners as well as p-Chekov is also presented. The deterministic stage of p-Chekov leverages the recent advances in obstacle-aware trajectory optimization to improve the original tube-based-roadmap Chekov planner. Through experiments in 4 common application scenarios with 5000 test cases each, we show that using sampling-based planners alone on high DOF robots can not achieve a high enough reaction speed, whereas the popular trajectory optimizer TrajOpt with naive straight-line seed trajectories has very high collision rate despite its high planning speed. To the best of our knowledge, this is the first work that presents such a systematic and comprehensive evaluation of state-of-the-art motion planners, which are based on a significant amount of experiments. We then combine different stand-alone planners with trajectory optimization. The results show that the deterministic planning part of p-Chekov, which combines a roadmap approach that caches the all pair shortest paths solutions and an online obstacle-aware trajectory optimizer, provides superior performance over other standard sampling-based planners' combinations. Simulation results show that, in typical real-life applications, this "roadmap + TrajOpt" approach takes about 1 s to plan and the failure rate of its solutions is under 1%. The risk-aware stage of p-Chekov accounts for chance constraints through state probability distribution and collision probability estimation. Based on the deterministic Chekov planner, p-Chekov incorporates a linear-quadratic Gaussian motion planning (LQG-MP) approach into robot state probability distribution estimation, applies quadrature-sampling theories to collision risk estimation, and adapts risk allocation approaches for chance constraint satisfaction. It overcomes existing risk-aware planners' limitation in real-time motion planning tasks with high-DOF robots in 3- dimensional non-convex environments. The experimental results in this thesis show that this new risk-aware motion planning and execution system can effectively reduce collision risk and satisfy chance constraints in typical real-world planning scenarios for high-DOF robots. This thesis makes the following three main contributions: (1) a systematic evaluation of several state-of-the-art motion planners in realistic planning scenarios, including popular sampling-based motion planners and trajectory optimization type motion planners, (2) the establishment of a "roadmap + TrajOpt" deterministic motion planning system that shows superior performance in many practical planning tasks in terms of solution feasibility, optimality and reaction time, and (3) the development of a risk-aware motion planning and execution system that can handle high-DOF robotic planning tasks in 3-dimensional non-convex environments.