Fixed the problem that the robot with respect to the rotational direction is oscillatory