Target Reaching Behaviour for Unfreezing the Robot in a Semi-Static and Crowded Environment