Reactive Navigation with Sensor Fusion and Geometric Optimization for Map-Based Path Planning. A Systematic Point-to-Point Navigation for AGV