The obstacles in the field of view of Kratos’ camera are detected by our vision algorithms and then represented as a cost map. The field of view of the camera is represented with a grid, with each box or “cell” corresponding to approximately a 10 cm x 10 cm square. Each cell is then assigned a cost based upon whether there is a detected obstacle occupying it. The more obstacle points in a cell, the higher its cost is.

below is a sample cost map generated from data taken outside of our research facilities. The robot is represented as the blue box near the center of the image.

After a cost map has been generated, Kratos plans a path through it to the next waypoint. This is accomplished by performing an A* graph search from its current position to its destination waypoint. In performing this search, multiple paths of the same length may be produced as solutions. Since the path is constantly being re-planned, there is a possibility that in the worst case, the planner would alternate between different paths of the same length. In that case, the robot would oscillate between the two paths, never converging to a single path, and ultimately crash into the obstacle.

To mitigate the possibility of this outcome, Kratos employs a “path straightness” heuristic, which measures how close nodes in the path are to the straight-line path between the current position and the destination waypoint. Therefore, path arbitration problems were always settled in favor of straighter paths. The image below shows an example of a path planned around an obstacle in the cost map.

Optimizations allowed our planner to run at between roughly 5 Hz and 12 Hz, depending on how cluttered of an obstacle field the robot was traversing.