Map occupancy grid optimized and obstacles expanded