How can a robot tell it is stuck?

When making a self-driving robot there are a lot of things to consider, such as:

• Figuring out where you are
• Deciding where to go next
• Working out what speed to move at

but how do you tell when things have gone a bit wrong?

With racing the most common problem is getting stuck, which can happen for any number of reasons:

• Running into the wall
• Crashing into another robot
• Running into an obstacle
• Leaving the track and getting lost

In Formula Pi hitting walls and other robots will happen so we need to be able to get the YetiBorg going again.

The easy part is knowing what to do, most of the time simply reversing slightly is enough to recover.
The challenge is to figure out we are stuck from what the Raspberry Pi camera can see.

What we can do is see if the camera image is changing.
If we are moving along the image should be different between the last frame and the next frame.
So if we take two frames:

we can then produce a difference image using Open CV:

import cv2

frame1 = cv2.imread('frame1.jpg')
frame2 = cv2.imread('frame2.jpg')

frameDiff = cv2.absdiff(frame1, frame2)
cv2.imwrite('diff.jpg', frameDiff)

with the result being:

So how does this difference image help us?

The basic idea is that two frames with movement will have a much larger difference then two frames from the same place.
This difference image is from two frames in the same place:

gives

We can simplify the image into a single number to check by taking the average value from the differences:

change =  frameDiff.mean()
print change

In the cases above we got about 4.22 for the image with movement and 1.97 for the image without movement.
A simple threshold can then be used to see if we are moving or not:

if change > 2.5:
    moving = True
else:
    moving = False

To avoid false positives we can simply check we get many images without enough change, then we know we are stuck.
In order to make things even more accurate we crop the image first so that we are only looking where the track should be.

Now that we know we are stuck we can reverse a little bit and get on with racing again :)