Connected invariant sets for high-speed motion planning in partially-known environments

Ensuring safety in partially-known environments is a critical problem in robotics since the environment is perceived through sensors and the environment cannot be completely known ahead of time. Prior work has considered the problem of finding positive control invariant sets (PCIS). However, this approach limits the planning horizon of the motion planner since the PCIS must lie completely in the limited known part of the environment. Here we consider the problem of guaranteeing safety by ensuring the existence of at least one PCIS in partially-known environments leading to an extension of the PCIS concept. It is shown, that this novel method is less conservative than the common PCIS approach and robust to unknown small obstacles which might appear in the close vicinity of the robot. An example implementation for loiter circles and power line obstacles is presented. Simulation scenarios are used for validating the proposed concept.