Skip to the lesson
FixTheRobot Explore Robotics / Navigation / M4 Sampling-Based and Kinodynamic Planning / M4-T2

Navigation · Theory · M4

M4-T2 — Collision Checking

The test a sampling planner runs millions of times: is this pose free, and is this edge free?

  • Lesson 2 of 6
  • Module 4: Sampling-Based and Kinodynamic Planning
  • Theory
  • About 9 minutes to read

Every sampling planner asks the same two questions over and over. Does the robot, posed here, touch anything? Does it touch anything while moving from here to there? Scout’s cart makes the first question harder than it looks, and a long edge makes the second question easy to get wrong. This lesson shows the exact tests, the cheap conservative ones, and the rule that decides how finely to sample an edge.

Learning objectives

After this lesson, you should be able to:

  • Check a rectangular footprint against a circular obstacle exactly, in the robot frame.
  • Use a bounding circle or a set of circles as a conservative first test.
  • Choose the sampling step for an edge check so that no obstacle can fit between two samples.
  • Explain why the heading must follow the path when the footprint is not round.

The intuition

A map tells the planner where the walls are. It does not tell it whether this particular robot, in this particular pose fits. The costmaps of module 2 answer that for a round robot by inflating the obstacles. A sampling planner works in continuous space and usually asks the geometry directly: take the footprint, place it, and test whether it overlaps an obstacle.

For a footprint that is not round there are two honest ways to do it. The exact way compares every part of the shape with every obstacle. The cheap way replaces the shape with something simpler that contains it, a circle or a few circles, and trusts only a “free” answer from that. Planners usually use the cheap test first and the exact test only when the cheap one is unsure.

An edge is a different problem: infinitely many poses lie along it. The standard answer is to check poses at regular spacing. Between two samples there is a gap, and the gap must be too small for an obstacle to hide in.

A footprint, its bounding circle and the samples along an edgeOn the left, Scout and its cart drawn as two rectangles inside a large circle that contains both. On the right, a straight edge with equally spaced samples passes close to a small pillar whose grown circle is larger than the spacing, so a sample lands inside it. body 0.60 × 0.40cart 0.65 × 0.70 bounding circle r = 1.11 m: safe but pessimistic pillar + robot radiusspacing ≤ grown radiussamples every s along the edge; one lands inside the grown disc
Left: a two-rectangle footprint and the circle that contains it. Right: samples along an edge. The spacing must be small compared with the obstacle grown by the robot radius.

The mathematics

Rectangle against circle

Let the robot be at pose (x, y, θ) with a rectangle [x₀, x₁] × [y₀, y₁] in its own frame, and an obstacle circle with centre c and radius r. Move the centre into the robot frame, clamp it to the rectangle and measure what is left:

Exact rectangle-circle test
l=R⊤(θ)⋅(c−(x, y))l=R^{\top}(\theta )\cdot (c-(x,\,y))
rotate by−θ\text{rotate by}-\theta
q=(clamp⁡(lx, x0, x1), clamp⁡(ly, y0, y1))q=(\operatorname{clamp}(l_{x},\,x_{0},\,x_{1}),\,\operatorname{clamp}(l_{y},\,y_{0},\,y_{1}))
closest point of the rectangle
collision⇔∣l−q∣≤r\text{collision}\Leftrightarrow \lvert l-q\rvert \le r

A footprint made of several rectangles (Scout plus a cart) collides when any rectangle does. For a rectangle against a polygonal obstacle, use the separating-axis test: two convex shapes are disjoint exactly when some edge normal of either shape separates their projections.

Bounding circles

A circle of radius equal to the largest distance from the robot’s reference point to any footprint corner contains the footprint at every heading. Testing it costs one distance per obstacle.

Bounding circle
rcirc=max⁡ over corners of xi2+yi2r_{\text{circ}}=\max \,\text{over corners of}\,\sqrt{x_{i}^{2}+y_{i}^{2}}

Its answer is one-sided: free for the circle means free for the footprint, but collision for the circle may be a false alarm. Several smaller circles along a long footprint cover it more tightly.

Checking an edge

A straight edge between configurations a and b is checked at samples a + (b − a)·i/n, for i = 0 … n, with n = ⌈length / s⌉. For a round robot of radius R and an obstacle of radius r, treat the obstacle as a disc of radius ρ = r + R. A sampled check can miss a disc only when the edge cuts it so near its boundary that the chord inside is shorter than the spacing s. The deepest undetected penetration is

Deepest undetected penetration
δmax⁡=ρ−ρ2−s24\delta _{\max}=\rho -\sqrt{\rho ^{2}-\frac{s^{2}}{4}}
s
spacing between samples along the edge.
ρ
obstacle radius plus robot radius: the radius of the forbidden disc.
δmax
the deepest the edge can enter the forbidden disc between two samples without a sample inside.

Rules of thumb follow: choose s no larger than the smallest ρ among the obstacles (then δmax is under ρ/7), and half of that if collisions matter more than speed. Collision-free results then mean collision-free to within a margin δmax, which can be absorbed in a safety margin.

Footprint along an edge

For a non-round footprint the sample at a point of the edge also needs a heading. A robot that follows its path points along the tangent. If the planner interpolates between two poses, both position and heading are interpolated. Using one fixed heading for all samples is a common mistake and checks the wrong region of space.

A worked example

Scout and the cart. The robot body is the rectangle [−0.30, 0.30] × [−0.20, 0.20] in its own frame. The cart is [−1.05, −0.40] × [−0.35, 0.35]: 0.65 m long and 0.70 m wide, wider than the robot. The far corner of the cart is at (−1.05, 0.35), so the bounding circle has radius

Bounding radius of Scout and cart
1.052+0.352=1.107 m\sqrt{1.05^{2}+0.35^{2}}=1.107\,\mathrm{m}

The body alone has rcirc = sqrt(0.30² + 0.20²) = 0.361 m. A check that uses only the robot’s centre with the radius of the body is therefore off by 0.75 m at the tail. Lab M4-L1 stage 1 starts exactly there.

One pillar, one pose. The robot is at (2, 1) heading 180° (so the cart is on the +x side), and a pillar of radius 0.2 m is at (3.0, 1.1). In the robot frame the heading 180° gives cos θ = −1, sin θ = 0, so l = Rᵀ · (1.0, 0.1) = (−1.0, −0.1). Clamped to the cart rectangle: q = (−1.0, −0.1). The distance |l − q| is 0, which is at most 0.2: the cart is touching the pillar. The robot body’s clamp gives q = (−0.30, −0.1) and a distance of 0.70 m: no contact. The circle around the body’s half-width would say free.

The spacing of an edge. A pillar of radius 0.1 m, a robot of radius 0.3 m, so ρ = 0.4 m. With s = 0.4 m the deepest undetected penetration is

Deepest miss
δmax⁡=0.4−0.42−0.22=0.4−0.3464=0.0536 m\delta _{\max}=0.4-\sqrt{0.4^{2}-0.2^{2}}=0.4-0.3464=0.0536\,\mathrm{m}

so a 5.68 m edge needs ⌈5.68 / 0.4⌉ = 15 intervals, 16 samples. Halving the spacing to 0.2 m takes 29 intervals, 30 samples, and brings δmax down to 0.4 − sqrt(0.16 − 0.01) = 0.0127 m. The three-sample check that tests only the ends and the middle has a spacing of 2.84 m, which is more than the pillar’s diameter and can step right over it.

Engineering implications and limitations

Two-stage checks

Test the bounding circle against a coarse obstacle structure first; only if it hits, run the exact test. Most poses are far from everything, and the cheap test removes them in a few operations. Spatial structures such as grids of obstacle indices, k-d trees or bounding volume hierarchies cut the number of obstacles tested.

Use the costmap when it fits

For a round or nearly round robot on a grid map, a pose check is a single lookup in the inflated costmap (module 2): no geometry at all. The footprint check on the real polygon is the second, more expensive test, often applied only to the final path.

Edge checks are the bill

The number of pose checks per edge is length / s, and a planner may check millions of edges. Check the long edges lazily (only when they might be part of the solution), check first the samples most likely to collide (the midpoint, then the quarter points), and stop at the first collision.

Margins

Add the safety margin once, to the robot radius or the obstacle size, and write down where. A margin added in both places closes doors (lesson M2-T2). Remember δmax from sampling when you set it.

Rotation too

If the planner connects two poses by turning on the spot, the footprint sweeps a disc, so check its bounding circle, or sample the rotation at an angular step that bounds how far a corner moves between samples: corner radius × Δθ.

Common misconceptions

“Checking the robot’s centre is enough if I inflate the map.”

Only if the inflation radius covers the whole footprint at every heading. A long cart behind a small robot is not covered by the robot’s inscribed radius (lesson M2-T2), and over-inflating to cover it closes doors.

“Three or five samples per edge are plenty.”

The spacing, not the count, matters. A fixed number of samples gives spacing proportional to the edge length, so the longest edges are checked the least well and can step over small obstacles.

“A bounding circle that says collision means collision.”

It means the footprint may collide. The circle is larger than the footprint, so it reports false collisions, and they close narrow doors. Follow up with the exact shape.

“The heading can be ignored for a robot with a small footprint.”

It can be ignored only if the footprint is nearly round. A rectangle 0.6 m long and 0.4 m wide has a half-diagonal of 0.36 m and a half-width of 0.20 m: a 0.16 m difference that decides whether a door of 0.9 m can be passed with the cart.

Engineering takeaways

  1. A rectangle and a circle overlap when the clamped centre, in the robot frame, is within the radius.
  2. A bounding circle never misses a collision but can report false ones; use it as the first test.
  3. Sample an edge at a spacing no larger than the smallest obstacle radius plus robot radius; the number of samples follows from the length.
  4. The deepest undetected penetration is ρ − sqrt(ρ² − s²/4): sampling is exact only up to this margin.
  5. A footprint that is not round needs a heading at every sample.

References and further reading

Collision-checking background in the planning literature and the sampling-based planning survey.

  • S. M. LaValle, “Planning Algorithms”, Cambridge University Press, 2006, chapter 5 (collision detection).
  • C. Ericson, “Real-Time Collision Detection”, Morgan Kaufmann, 2005 (separating axis tests, bounding volumes).
  • S. Macenski, F. Martín, R. White and J. Ginés Clavero, “The Marathon 2: A Navigation System”, IEEE/RSJ IROS, 2020.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

A footprint has corners at (±0.5, ±0.3) in the robot frame. What is the radius of its bounding circle?

Explanation

sqrt(0.5² + 0.3²) = sqrt(0.34) = 0.583 m.

Question 2

An obstacle of radius 0.15 m and a robot of radius 0.25 m. What is the largest sample spacing that keeps the deepest undetected penetration below 0.02 m?

Explanation

ρ = 0.40 m. We need 0.40 − sqrt(0.16 − s²/4) ≤ 0.02, so sqrt(0.16 − s²/4) ≥ 0.38, so s²/4 ≤ 0.0156 and s ≤ 0.25 m.

Question 3

A path turns by 90° at a corner. The footprint is checked with the heading of the first segment on the whole path. What can go wrong?

Explanation

After the corner the real footprint points in a new direction, so the region checked is the wrong one. A long cart can sweep into an obstacle that was never tested, while a free pose may be reported as colliding.

What’s next

Next: the two basic sampling planners, PRM for many queries and RRT for one.

Practice this lesson

Collision-Check a Footprint

Scout's cart sticks out wider than the robot: check the whole footprint, not just the centre.

Next lesson

M4-T3 — PRM and RRT

Build a roadmap and a tree, and explain probabilistic completeness and the Voronoi bias of RRT.

Nothing below is required to finish this lesson.