5.5.1.1. Go To Goal

The Go-to-goal behavior uses the robot’s pose in the global frame from our odometry calculations and the (x, y) coordinate for the location of a goal or target, X_g. Simple trigonometry calculations give us the target direction, \phi. The robot heading is then simply \alpha_g = \phi - \theta.

When the robot is close to the goal, it may be desired for the robot to go slower. We will consider that we have arrived at the goal when the pose indicates that we are within a small distance of the goal, \left\| X_g - X \right\| < \epsilon.

The robot will sometimes drive slightly past the goal before stopping. A simple way to detect that it has driven past the goal is that \alpha_g will become close to \pi rather than close to zero, as it should be when it is approaching the goal. If no adjustment is made, the steering controller will attempt turn the robot around to go back to the goal, which gives an undesired behavior of spinning. It would be better to back into the goal position. Rather than trying to modify the steering controller to deal with this scenario, it is simpler to just change the inputs to the steering controller to get the desired result. Since we are using our forward driving controller, it might seem like we should orient the robot towards \alpha' = \alpha + \pi and drive in reverse (V' = -V). However, this will not work because when we reference the velocities of the right and left wheels, we do so with regard to driving forward. We would need to switch which velocity goes to each wheel to drive in the desired direction. The correct solution is to set \alpha' = \pi - \alpha and V' = -V.