Simple Path Collision Checking in 2D
You've planned a path that goes from A to B.
Maybe it's direct. Maybe it's limited by the maximum steering angle of your robot and is thus curved.
But how can you check if it's collision free?
For that, knowledge of the environment in which the robot moves is necessary.
From sensors, a map of the environment in which the robot is operating can be built.
In 2D, objects within the environment could be represented by boxes or to encapsulate more details: polygons.
Polygons are simple enough for computational efficiency and complex enough to represent various shapes.
So let's recap. You have a path and polygons that represent obstacles.
How can you check if the path collides with obstacles within the map?
There are two possible approaches: analytical versus discrete.
For the analytical solution, a simple strategy for convex polygons would be to use the equation of the path and each segment of the polygon represented as a line to check for intersections.
For the discrete solution, the strategy is to discretize the path for a given number of points and test if any of the points belongs to the polygon.
Let's take a simple example: if the path is a line like y = 3x + 2 with xstart = 1 and xend = 3, you can sample the path from start to finish for N points. At x0 = 1 you would compute y0, at x1 = 1.02 you get y1 = 5.06, etc…
You will end up with N 2D points that are uniformly spaced in x along the path from start to finish.
Next, collision tests can be made for each of these points that represent the path with obstacles.
The problem morphs into assessing if a point is inside a polygon.
Let's assume the polygon is convex.
Given a line of the polygon (A→B) and a point (C), two vectors can be built: AB and AC.
The point C is inside the polygon when it's on the same side of every polygon edge.
This can be computed with the cross product of the two vectors.
In a standard right-hand coordinate system (x right, y up): positive means left side, negative means right side.
For a clockwise polygon, the point must be on the right side (negative) of every edge.
Drag the point below to see which side of the segment it falls on.
When traveling clockwise along the segments of the convex polygon, if all cross products share the same sign, the point is inside.
If any cross product has a different sign, the point is outside.
If even one edge test returns left side, the point is outside.
Drag the point below around the pentagon to see the test in action. The animation restarts each time you move it.
Another technique that is generalizable to concave polygons is ray-casting.
The idea is to cast a ray from the point and count how many polygon segments it crosses.
If the ray crosses an odd number of segments, the point is inside the polygon.
If the ray crosses an even number, it went in and out, and the point is outside.
Drag the point below to see ray-casting in action on a concave polygon.
Now going back to the main problem, knowing if a discretized path collides with a polygon involves doing the above tests repeatedly for each point along the path.
Bonus: If a multi-core computer is used, it's possible to parallelize the computation by delegating a subset of the points to each core.
Below you can choose the number of cores. The 100 sample points are divided among them and processed simultaneously. Watch how more cores speed up the check.
One caveat of the discretization approach is that if it's not dense enough, small obstacles might be missed!
Try reducing the number of sample points below and see how the obstacle slips between them undetected.
That's it. A simple geometrical collision checking technique for robotics.
Discretize the path. Test each point against polygon obstacles. Parallelize if you can. And always check your sampling density.