Safety-aware autonomous robot navigation, mapping and control by optimization techniques