Smooth path planning with safety margins using Piece-Wise Bezier curves