Path planning algorithm to safely navigate a vehicle around highway while changing lanes and limiting acceleration and jerk.