# Path Planning and Path Tracking

We currently have two types of path planning implemented into our AI software. The first is a quite simple, effective but slow point by point calculation. The second is a method called 'Bang Bang Trajectories'. The Bang Bang Trajectories have been improved during 2022 and we strive to fully switch to this method in the future.

## Simple Path Planning

The old path planning calculates a path by looking for a distance optimal path. It tries this by drawing a line directly from the robot to the target and then checking for collisions. If it finds a collision it will draw two lines to a point to either side of the collision and then to the target and check again. It does this recursively until it finds a valid path or a specified maximum number of iterations is reached. When not finding a path it returns an empty vector.

## Bang Bang Trajectories

The new path planning tries to find a path that is time optimal. It does this with a technique called BBT (bang bang trajectories). This technique first calculates for the x and y direction separately for how long a certain acceleration is needed to arrive at the correct x or y coordinate. The calculation takes into account the current velocity, position, maximum velocity and maximum acceleration (and deceleration).

The trajectory in one dimension can consist of three types of parts: a maximum acceleration, constant maximum velocity and a maximum deceleration. This ensures that the wanted x or y position is reached as quickly as possible. So when looking at the velocity graph of a trajectory it either has a triangular or a trapezoidal shape.

This trajectory is only for one dimension though. As the robot has a maximum acceleration, the acceleration needs to be “distributed” over an acceleration in the x and y direction. By “giving” most of the acceleration to x for example, the duration of the trajectory in the x direction becomes shorter but the trajectory in the y direction becomes longer. By plotting the distribution of the acceleration on the horizontal axis and the time that the trajectory takes on the vertical axis a graph can be made. By finding the point at which the two lines intersect, you find the trajectory that takes the shortest time to get to the target location while also taking into account the limitations of the robot.

### Avoiding Obstacles

The algorithm first checks if there are obstacles on the trajectory. It does this by looking at the path approximation and checking each point for a collision. After all different kinds of collisions are checked it will return the location of the first collision point and obstacle position. It checks the “path approximation” as the BBT is saved as a continuous function and needs to be made discrete to check positions on the path for collisions. The algorithm will take the following steps to avoid the collision and find a correct path: