Safe motion planning with environment uncertainty