Gaussian belief space planning with discontinuities in sensing domains

Discontinuities in sensing domains are common when planning for many robotic navigation and manipulation tasks. For cameras and 3D sensors, discontinuities may be inherent in sensor field of view or may change over time due to occlusions that are created by moving obstructions and movements of the sensor. The associated gaps in sensor information due to missing measurements pose a challenge for belief space and related optimization-based planning methods since there is no gradient information when the system state is outside the sensing domain. We address this in a belief space context by considering the signed distance to the sensing region. We smooth out sensing discontinuities by assuming that measurements can be obtained outside the sensing region with noise levels depending on a sigmoid function of the signed distance. We sequentially improve the continuous approximation by increasing the sigmoid slope over an outer loop to find plans that cope with sensor discontinuities. We also incorporate the information contained in not obtaining a measurement about the state during execution by appropriately truncating the Gaussian belief state. We present results in simulation for tasks with uncertainty involving navigation of mobile robots and reaching tasks with planar robot arms. Experiments suggest that the approach can be used to cope with discontinuities in sensing domains by effectively re-planning during execution.