Consider implementing Craig Reynolds path following steering behaviour

[MHeasell]

The current behaviour of path following where we try to pass through each point successively sometimes means that units miss the point and loop back around to get it, when they would be better off continuing down the path. The behaviour proposed by Craig Reynolds would solve this problem.

See https://www.red3d.com/cwr/steer/