Control Synthesis and Classification for Unicycle Dynamics using the Gradient and Value Sampling Particle Filters

Value functions arising from dynamic programming can be used to synthesize optimal control inputs for general nonlinear systems with state and/or input constraints; however, the inputs generated by steepest descent on these value functions often lead to chattering behavior. In [Traft & Mitchell, 2016] we proposed the Gradient Sampling Particle Filter (GSPF), which combines robot state estimation and nonsmooth optimization algorithms to alleviate this problem. In this paper we extend the GSPF to velocity controlled unicycle (or equivalently differential drive) dynamics. We also show how the algorithm can be adapted to classify whether an exogenous input—such as one arising from shared human-in-the-loop control—is desirable. The two algorithms are demonstrated on a ground robot.