EPISODE TRANSCRIPT
Robotics, Vision and Control (Peter Corke) — Part 2 of 5: Mobile Robotics
Automatically transcribed from the episode audio. Some words and names may be inaccurate.
Listen to the episodeDownload textDownload captions
So back in 2007, there was this robotic vehicle named Boss that completely made history. It was built by the Tartan racing team over at Carnegie Mellon, and it actually won the DARPA Urban Challenge. Right, which was a massive deal at the time. Oh. Absolutely huge. It successfully navigated this really complex, like 60 -mile mock city course. It was obeying traffic laws, avoiding other robots, doing all the things you'd expect a human driver to do. Yeah, and Boss wasn't some tiny little lab rover either. No, not at all.
It was a heavily modified Chevrolet Tahoe. We're talking a 5,000 -pound truck with four massive tires just packed to the brim with lasers, radars, and computing clusters. But here's the crazy part. Mathematically speaking, inside its digital brain, Boss genuinely believed it was riding a bicycle. Which just sounds so absurd when you say it out loud. Right? It makes no sense at first glance, but welcome to the deep dive. We are so glad you were here with us today. We are continuing our exploration of the Marginalia series on Peter Corke's text, Robotics, Vision and Control, Fundamental Algorithms in Python.
The 2023 edition, just to be precise. Yes, exactly. This is part two of five, mobile robotics. And today we're starting with this really strange paradox. We had to figure out why reducing a two -ton four -wheeled truck to a mathematical unicycle or bicycle is basically the only way to keep it from crashing. It really does seem completely counterintuitive when you first look at the engineering of it all. You have this massive complex mechanical system. And I think your first instinct as a programmer is to model every single moving part. Right, you'd want a variable for the front left wheel, the front right wheel, the rear suspension. Exactly, the chassis, you roll, all of it. But Peter Corke introduces this kinematic bicycle model really early on to demonstrate a foundational rule of mobile robotics. And that rule is that mathematical simplification is often the only path to actual real -time control. You just have to strip away the physical noise. Yeah, you strip it away to find the core geometric truth of how a vehicle actually moves through space. Now, if you were with us for part one, remember we spent all our time building those mathematical foundations. We looked at how to represent position, orientation, reference frames, trajectories. The pure geometry of it all. Exactly.
We essentially built the pure geometry of how things exist and move in abstract space. But, you know, abstract space doesn't have friction. Abstract space doesn't have flat tires or dirty floors or potholes. Unfortunately not. Right. So today we are taking those abstract reference frames and we are literally putting them on wheels. We're covering chapters four, five, and six today. We have to start by figuring out how a robot physically interacts with the ground, then how it plans a path, and finally how it handles the uncertainty of where it actually is. It's really a logical progression. Mobility, then navigation, then localization. So let's jump straight back to Boss and this bicycle model from chapter four.
Why on earth do we compress a four -wheeled car into a two -wheeled bike? Well, the answer lies in how a car actually steers, which is surprisingly complex when you really break it down. When a four -wheeled vehicle turns a corner, the two front wheels cannot point in the exact same direction. Wait, really? I just assumed they stayed perfectly parallel. A lot of people assume that. But if they were perfectly parallel, the car would literally tear its own tires apart during a turn. Because the inside wheel is traveling on a tighter, smaller circle than the outside wheel, right? Yes, exactly.
The inside wheel has to navigate a much shorter radius, so it has to be turned at a slightly sharper angle than the outside wheel. If it isn't, one of those front tires is just going to drag sideways across the pavement. Ouch. It sounds terrible for the hardware. It is. And this mechanical problem was actually solved back in the early 19th century with something called Ackerman steering geometry. It uses this clever trapezoidal linkage between the front wheels. So as you turn the steering wheel, the inner tire naturally angles more aggressively than the outer tire. Right. So both front wheels are effectively tracing concentric circles. They are orbiting a shared center point. OK, I can visualize that. In robotics, we call that the instantaneous center of rotation, or the ICR. Imagine drawing an invisible line extending straight out from the rear axle of the car. Okay, got it. Then draw another line extending out from the front axles, perpendicular to the angle of the turned wheels. Because of that Ackerman linkage, the lines from the front left wheel and the front right wheel will intersect the rear axle line at the exact same point in space. And that intersection is the ICR. Exactly.
And as long as the vehicle's moving at low speeds and the tires aren't slipping, the entire car is essentially pinned to that invisible point. Like it's swinging around it like a tetherball? That's a perfect analogy, yeah. Which means we don't actually need to model the left and right wheels independently in our code. Because they are mathematically locked into orbiting that same exact ICR, we can just collapse the entire front axle into a single virtual wheel sitting right in the dead center of the car. Oh, wow. And I assume we do the same for the rear axle.
We do. The complex four -wheeled Ackerman geometry collapses perfectly into a two -wheeled bicycle. Okay, so the math gets infinitely cleaner. We can describe the state of this entire vehicle on a 2D plane using just three numbers, right? It's x -coordinate, it's y -coordinate, and it's heading angle. Basically, which direction it's pointing. Just those three, yep. And we only have two inputs to worry about. The forward velocity of the rear wheel and the steering angle of the front virtual wheel. But, and Cork makes a huge deal out of this, this simplification exposes a massive headache. Even though the robot lives in a 2D world where it can theoretically move forward, sideways, and rotate, it doesn't actually have the freedom to do all three. Right. Quirt calls this a non -holonomic constraint. Non -holonomic constraint. Yes. Let's unpack that because it sounds intimidating. It's just a fancy term for a restriction on the derivative of the robot's configuration, meaning it's a restriction on its velocity, but not necessarily a restriction on its final position. Okay, so for our mathematical bicycle, what is the actual constraint? The constraint is simply that the wheels cannot slide laterally.
The velocity along the vehicle's y -axis, sideways, must always be mathematically zero. I always picture this like trying to slide a train off its tracks. The wheels dictate a strict one -dimensional line of motion at any given microsecond. Yes, exactly. Like the train can go forward along the rails or backward along the rails, but it cannot just effortlessly slide three feet to the left onto an adjacent track. The physics of the wheel absolutely forbid it, and this creates what control theorists call an under -actuated system. The configuration space has three degrees of freedom. X, Y, and heading. But you only have two actuators. The motor driving the wheels and the motor turning the steering column. Right.
You are missing a lateral actuator. You have no motor that pushes the car perfectly sideways. Which is exactly why parallel parking takes so long for human drivers. If I want my car to move three feet to the right into a parking space, I can't just press a move right button. You have to perform a really complex sequential dance. Yeah, I pull forward, turn the steering wheel all the way to the right, reverse, stop, turn the steering wheel all the way to the left, reverse again. I am basically trading time and complex maneuvers to trick an under -actuated system into achieving a lateral displacement. Exactly.
And you are exploiting the fact that the constraint is non -holonomic. If it were a holonomic constraint, it would mean you could literally never reach that parking spot. Period. Oh, interesting. So a non -holonomic constraint just means you can't reach it instantaneously. Right. You have to integrate your two available controls over time to arrive at the desired three -dimensional state. Okay. So how do we actually write code to make this mathematical bicycle do what we want? Cork uses a Python simulation package called BDSim, right? Block diagram simulation. He does. He uses BDSim to build out four specific controllers.
Let's start with the simplest one, driving to a point. Let's say we have a robot at coordinate 0, 0, and we want it to go to a target at 10, 10. How does it get there? We use proportional control. It's a foundational feedback mechanism. The robot calculates the Euclidean distance to the target. Let's call that distance rho. It multiplies rho by a gain factor to set its forward velocity. So as the robot is far away, rho is a large number, so it drives fast. Exactly. And as it approaches the target, rho naturally shrinks towards 0. and the velocity slows down, bringing it to a nice, smooth stop. And what about the steering? Simultaneously, it calculates the angle to the target and sets its steering angle proportional to the error between its current heading and the target heading. Now, there is a very specific, almost pedantic detail Cork points out when calculating that angle. You can't just use standard high school trigonometry. You can't use the standard inverse tangent function A tan. You have to use A tan2. Oh, this is such a classic trap for beginners.
Why does a robot care about a slightly different trig function, it's just an angle? Because standard ATAN takes a single argument, the ratio of the y distance divided by the x distance. The mathematical flaw there is that division destroys information about the signs of the coordinates. Okay, walk me through an example of how that breaks the robot. Sure. Let's say your target is at negative 5 on the x -axis and negative 5 on the y -axis. You divide negative 5 by negative 5. The negatives cancel out, leaving a positive 1. OK, basic algebra.
But then the ATAN function looks at that positive 1 and calculates an angle of 45 degrees, which is in the upper right quadrant. But the target is actually behind the robot and to the left, in the lower left quadrant. Exactly. So the robot would confidently steer in the exact opposite direction of the goal, thinking it was doing a great job. That is hilarious. And terrible. So how does ATAN 2 fix it? The E10 -2 function solves this by taking the y and x distances as two separate arguments, rather than dividing them first. It preserves the negative signs, checks exactly which of the four quadrants the target resides in, and returns the unambiguous globally correct angle. It's a tiny detail in the Python code, but if you get it wrong, your robot will relentlessly drive away from its destination.
It's those little translations from pure math to computer science that always trip you up. All right, so that's driving to a point. The second controller, Cork outlines, is driving along a line. We aren't going to a single coordinate anymore. We are locking onto an infinite mathematical line drawn across the plane. And a single proportional controller won't work here. You need a combined control law. Why is that? Because if you only tell the robot to minimize its perpendicular distance to the line, it will simply turn 90 degrees, drive straight at the line, hit it, overshoot because it has momentum. realize it's off the line again, turn around, and wildly oscillate back and forth across the line. No, it's just zigzagging forever. So you have to introduce a second objective. You do. You have to minimize the distance to the line and simultaneously minimize the difference between the robot's heading and the line's angle. Ah, so you steer proportional to the distance error plus the heading error.
Exactly. The result is this beautiful asymptotic curve. The robot sweeps gently toward the line and smooths its steering angle so that by the time its distance error reaches zero, its heading error is also exactly zero, and it locks perfectly onto the track. That makes perfect sense. But infinite straight lines don't really exist in a warehouse or a city street, which brings us to the third controller, driving along a path. Cork calls this algorithm pure pursuit. Yes, pure pursuit. You'll see this everywhere in mobile robotics. So let's say we have a winding arbitrary path made of hundreds of waypoints. How does pure pursuit tackle it? Pure pursuit is one of the most widely used tracking algorithms because it is just incredibly robust and computationally cheap. The mechanism is simple. You place a virtual target point on the path ahead of the robot.
I always picture this as the old carrot and donkey routine. Yes. The robot is the donkey, the path is the ground, and we are dangling a virtual carrot slightly out of reach in front of it. And the robot just uses that first controller driving to a point to chase the carrot. And the crucial variable in that algorithm is the length of the stick holding the carrot. The distance from the robot to that virtual target point is called the lookahead distance. So as the robot moves forward, the carrot continuously slides further down the path, always maintaining that exact lookahead distance. Right. It's constantly updating. Let's play with that variable, because I think it highlights the physics perfectly.
What happens to the math if I make the stick incredibly short? Like, if the carrot is essentially dangling right in front of the robot's front bumper. The robot becomes pathologically reactive. Okay. Because the target point is so close, any slight curve or tiny deviation in the path causes a massive, immediate change in the required steering angle. That's like a student driver staring at the pavement directly in front of the hood, over -correcting every half second. Exactly. The robot will try to execute these violent, high -frequency steering corrections. The tires will scrub, the chassis will shake, and if the robot has any significant mass or speed, those rapid oscillations will throw it into instability. It might even flip. Yikes. Okay, so obviously we don't want a short stick, but what if I lengthen the stick?
What if I push the carrot way out, say, 20 meters ahead? Well, the steering becomes buttery smooth. But there's a catch, right? There's a big catch. It becomes smooth, but it becomes wildly inaccurate. If the path takes a sharp 90 degree left turn, but your look ahead distance is 20 meters, the virtual carrot will round that corner long before the robot does. Oh, I see. The robot will see the carrot off to the left, calculate a straight line proportional response, and simply drive diagonally right across the grass to get to it.
It completely cuts the corner. It completely ignores the path. So tuning the look -ahead distance is this delicate negotiation between tracking precision and dynamic stability. High -speed robots absolutely require longer look -ahead distances to maintain physical stability, which means their paths must be designed with gentle sweeping curves so they don't cut those corners. That physical constraint leads us right into Cork's fourth controller, driving to a configuration. This is essentially the parking problem. We don't just want to arrive at a coordinate. we want to arrive there with a very specific heading.
And this forces us to confront that non -holonomic constraint directly. To solve the parking problem mathematically, Cork performs a really fascinating transformation in the code. He writes the control law not for a bicycle, but for a unicycle. A unicycle. So just a single wheel that can drive forward and spin in place. Right. A unicycle model takes two commands, linear velocity and angular velocity. The math for driving a unicycle to a specific configuration is well established and highly elegant. You basically calculate the required linear speed and the required spin rate to gracefully spiral into the parking spot. But our robot isn't a unicycle. It's a bicycle. Boss is a Tahoe, not a unicycle. True. So we have to add a final mathematical matrix transformation at the very end of the loop. We take the desired angular velocity of the virtual unicycle, and we use the physical wheelbase length of our bicycle to calculate the exact front wheel steering angle required to achieve that same rotation rate.
Wow. We pretend to be a unicycle to figure out the trajectory, and then we magically translate that trajectory into steering wheel commands for the bicycle. It is a brilliant sleight of hand. It really is. But honestly, as I was reading this section, watching cork bend over backward to accommodate this nonholonomic constraint. I just kept asking myself, why are we fighting this? What do you mean? Like, if car -like steering makes the math so incredibly difficult and the maneuverability so limited, why don't we just design wheels that can move sideways? Well, you aren't the first person to ask that. Engineers have spent decades designing alternative locomotion to escape the tyranny of Ackerman steering. Cork actually details two major alternatives in this chapter. I assume. The first is differential steering, commonly called skid steering. Think of a bulldozer, a military tank, or even a standard Roomba vacuum cleaner. Right. There is no steering wheel on a Roomba. The wheels on the left side are locked together, and the wheels on the right side are locked together. And the control math for that is blissfully simple. The forward speed of the robot is just the average speed of the left and right wheels. The rotation rate of the robot is proportional to the difference in speed between the left and right wheels. So if you drive the left wheels forward at one meter per second and the right wheels backward at one meter per second, the vehicle will just spin in place around its geometric center. Exactly. OK. It can turn on a dime. It can change its heading while its linear velocity is strictly zero. That's amazing. Why don't cars use that? Because the physical cost is severe. Because the wheels are locked in a parallel configuration, they physically resist spinning in place. To rotate the vehicle, the motors have to generate enough torque to overcome the lateral friction of the rubber dragging against the floor. Oh, the tires literally have to skid sideways. Hence the name skid steering.
And the skidding is highly non -deterministic. Depending on whether the floor is dusty, wet, or carpeted, the amount of force required to skid changes dramatically. Though it's unpredictable. highly unpredictable. It causes massive wear on the tires, it drains the battery quickly, and most critically, it completely destroys your odometry, which we will see becomes a huge problem later. You never truly know how far you've turned because you are constantly slipping. Okay, so skid steering gives us maneuverability but ruins our precision. What about a truly holonomic system, like a wheel that can roll sideways without skidding at all? Cork introduces meconum wheels here, which just look like something out of a sci -fi movie. They really do. They are standard wheels, but around their circumference they have a series of passive barrel -shaped rollers mounted at a strict 45 -degree angle. I've seen these. They use them on the Kuka U -Bot, right? Yes, the Kuka U -Bot is famous for them. Mechanum wheels are an absolute masterpiece of vector mathematics.
When a standard wheel applies torque, the friction force vector points straight ahead. But because a mecanum wheel contacts the ground through those 45 degree rollers, the force vector is applied diagonally. So if you put four of these wheels on a rectangular chassis, you have four diagonal force vectors pointing in different directions. And by independently controlling the speed and rotation direction of each of those four wheels, you can add and subtract those vectors. How does that work in practice? Well, if you want to move forward, you just spin all four wheels normally.
The lateral sideways components of those diagonal vectors cancel each other out and the forward components add together. The robot goes straight. But if you want a straight sideways, like perfectly sideways, you drive the wheels on one side toward each other and the wheels on the other side away from each other. The forward and backward vectors completely cancel out. and the lateral vectors compound. The entire robot glides smoothly to the right or the left without changing its heading at all and without skidding a single tire. That is just incredible. It is a perfectly holonomic system. You have three degrees of freedom and you can actuate all of them simultaneously. It is the holy grail of maneuverability. So I have to ask again. Why isn't every robot in the world riding on mech and wheels? Why isn't my Honda Civic equipped with them to make parallel parking obsolete? because the physical constraints always demand payment. Mechanum wheels rely on absolute precision in how those rollers contact the ground.
The contact patch of a roller is tiny compared to a standard tire. Furthermore, for the vector cancellation to work perfectly, all four wheels must carry an equal load and maintain continuous, firm friction with the surface. Okay, so what happens if the robot drives over a small pebble or one wheel dips into a pothole? The force vector from that specific wheel disappears. The entire mathematical balance collapses, and the robot veers wildly off course. Oh, wow. So they're incredibly fragile in the real world. Very much so. Mechanum wheels require pristine, flat, hard surfaces. They are exceptional for indoor factory floors, semiconductor fabs, or Amazon fulfillment centers. But if you take a mechanism wheel outside on dirt, grass, or uneven asphalt, it is utterly useless. The roller's jam, the traction fails, and the math breaks down. And that messy reality, the physical world, is exactly why the non -holonomic car -like wheel remains the dominant paradigm for mobile robotics. It is mathematically annoying, but it's physically robust. Exactly. You can drive a Tahoe through the mud, but you can't drive a U -Bot through the mud. All right, so we have modeled the physics. We understand the wheels, the constraints, and the low -level controllers required to move the chassis. We've officially conquered Chapter 4, but moving isn't the same thing as navigating. Not at all. If I put a blindfold on, I can technically walk, but I don't know where I'm going. We have to figure out how the robot decides what path to take through the world, which brings us to Chapter 5, Navigation. And Quirk starts this chapter by stripping away almost all the computing power and showing us navigation at its most primitive reactive level, Breitenberg vehicles. Ah, Valentino Breitenberg. He was a cyberneticist who showed how complex, seemingly intelligent behavior could arise from incredibly simple neural wirings. A Breitenberg vehicle doesn't have a map. It doesn't use coordinate geometry. It just maps sensor inputs directly to motor outputs. Cork outlines a vehicle designed to seek out a light source or maybe a chemical trail. It has two sensors. One mounted on the front left, one on the front right. Bilateral sensing, just like biological eyes. Or the antenna on a moth. And the wiring is beautifully simple.
You take the signal intensity from the right sensor, and you feed it directly into the speed controller for the left wheel. Wait, you cross the wires? You cross the wires. The cross -wiring is what creates the behavior. If a light is shining from the right side of the room, the right sensor receives a stronger signal than the left sensor. Because it is wired to the opposite side, the left wheel receives a command to spin faster than the right wheel. And if your left wheel is spinning faster than your right wheel, the chassis naturally turns to the right. Yes. The robot automatically arcs toward the light. As the robot turns and faces the light dead on, the sensors receive equal signals, the wheels spin at the exact same speed, and it drives straight ahead toward the goal. That is brilliant! Now, the behavior is emergent. Without a single line of path planning code, without any knowledge of its XY position in the room, the robot will gracefully seek out the global maximum of the light source. It even slows down as it gets closer, assuming the signal scales appropriately, and just parks itself directly under the bulb. It mimics biological tropism perfectly. It's mesmerizing to watch, because it looks like it's thinking, like it has intent. But Cork points out that the illusion breaks the second you introduce an obstacle. Oh, it fails immediately. If you place a brick wall right between the Breitenberg vehicle and the light, it'll just drive straight into the wall. The sensors are shouting, go forward to the light, and the motors will grind against the brick until the batteries die. Because reactive navigation is fundamentally memoryless. It only knows the gradient of the signal at the current instant in time. It cannot formulate a strategy to go away from the goal temporarily in order to navigate around a U -shaped obstacle. It's too simple. To solve a maze, you cannot just react. You need state. You need memory. You need a map. Which brings up the granddaddy of map -based navigation, Shaky the robot, built at Stanford in the late 1960s. Shaky was the first robot to actually fuse vision, mapping, and planning.
It would look at a room full of blocks, build a digital representation of the space, and then calculate a geometric path to its goal. Corky explains that there are two fundamental data structures we use to give a robot a map. Mathematical graphs and occupancy grids. And they represent space in two completely different ways. A mathematical graph in the context of computer science is a collection of nodes connected by edges. Think of a highway map or a subway system. So the nodes are intersections and the edges are the roads between them. Exactly. Each edge has an associated cost. maybe the distance or the time it takes to traverse. Graph search algorithms like Dijkstra's algorithm or A -star simply explore the network to find the sequence of edges that results in the lowest total cost from the start node to the goal node. Graphs are incredibly efficient, but they require you to already know the topology of the space. You have to know where the roads are beforehand. Right. But if I drop a robot into an open warehouse or a forest, there are no defined roads. The robot can theoretically drive anywhere that isn't a tree or a box. That's where we need an occupancy grid. Yes. An occupancy grid discretizes continuous space. You overlay a giant piece of virtual graph paper across the environment. You break the world down into tiny squares, maybe 10 centimeters by 10 centimeters.
And you just assign a binary value to each cell, right? Yep. It is either free space, meaning the robot can safely drive there, Or it is occupied, meaning there is a wall or an obstacle. OK, so now we have a massive grid of ones and zeros. How do we actually find a path through it? Quark explains a technique called the distance transform. And honestly, this was one of the most elegant concepts in the entire chapter. It really is beautiful math. The distance transform is a way front expansion algorithm. You start by identifying the specific cell in your grid that represents your goal destination. You assign that cell a value of zero. OK. Goal is zero. Then you look at all the immediate neighbor cells, the ones adjacent to the goal. Assuming they are free space, you assign them a value of one. So you are basically measuring the distance to the goal in units of grid cells. Exactly. Then you look at the unvisited neighbors of those cells and assign them a two. You iterate this process over and over. It's mathematically identical to dropping a pebble into a still pond. A way front of numbers ripples outward from the goal. And when the wayfront hits an obstacle cell. It can't pass through, so it flows around the edges of the obstacle, filling every single pocket of free space in the map, until the wayfront finally washes over the specific cell where the robot is currently sitting. And once that grid is populated with those rippling numbers, the planning is trivial, right? The robot doesn't even have to think.
It does no thinking at all. No. It just looks at the eight cells immediately surrounding it, finds the cell with the lowest number, and moves there. It just rolls. downhill along the gradient, 50, 49, 48, flowing around corners taking the absolute shortest path straight to the zero at the goal. It is mathematically guaranteed to find the shortest path if one exists. But, there's always a but. It assumes a binary world. It assumes all free space is equally desirable. In reality, a warehouse might have a smooth concrete aisle and a cluttered bumpy storage area. Both are technically free space. But the bumpy area requires way more energy and time to traverse. We need a way to encode the difficulty of the terrain.
Which introduces D -star, or dynamic A -star. Okay. This is an essential upgrade to the basic grid planner. D -star transitions from a simple distance metric to a traversability cost. We assign a weight to every single cell. Okay, give me an example of how that changes the math. Sure. The smooth concrete might cost one unit of energy to cross. The bumpy storage area might cost five units. The algorithm calculates the wavefront by accumulating these costs. So the robot might actively choose a path that is physically longer, in actual distance, but mathematically cheaper in energy because it goes around the high cost terrain. That is really smart. But the real power of D -Star is the dynamic part, right? A standard distance transform is incredibly brittle. If the robot calculates its beautiful optimal path, starts driving, and suddenly its lidar detects that someone left a forklift parked right in the middle of the aisle. The path is blocked.
Right. And in a naive algorithm, discovering a new obstacle means you have to throw away the entire map, reassign the forklift cells as occupied, and run the entire wavefront expansion again from scratch. Which takes forever. If your grid has a million cells, doing that recalculation every time you see a new box is computationally ruinous. The robot would just freeze in place for five seconds every time a person walked by. So how does D -Star avoid starting over? Under the hood, as D -Star builds the initial path, it maintains a network of back pointers. Every cell remembers which neighbor it received its cost from. That's clever. It is. When an unexpected obstacle appears, the robot just updates the cost of those specific blocked cells to infinity.
Then it uses those back pointers to trace the shock wave of that change outward, only recalculating the costs of the local cells that are directly affected by the blockage. So the vast majority of the map remains completely untouched. Exactly. D -STAR incrementally repairs the path on the fly, making it incredibly efficient for real -world changing navigation. It's an elegant solution for a 2D warehouse floor. But I kept thinking about scale while reading this. If I want a robot to navigate across an entire university campus, and I'm using a 10 -centimeter grid to capture the detail of the doorways and curbs, that matrix is going to contain hundreds of millions of cells.
Even D star is going to choke on that much memory. You are hitting the curse of dimensionality. Grid -based planners scale terribly. The computational cost grows exponentially with the size of the environment and the resolution of the grid. If you move from a 2D floor plan to 3D space, say path planning for a drone flying through a city, a volumetric grid becomes instantly impossible to compute in real time. So we have to abandon the grid entirely. We do. Cork introduces a completely different approach to path planning called PRM, probabilistic roadmaps. And the philosophy here is that we shouldn't try to map every single inch of the world. We should just sample it.
Just sample it. How does that work? PRM relies on random sampling within the configuration space. Instead of dividing the map into millions of tiny squares and checking every single one, the algorithm just generates random arbitrary coordinates. Like throwing darts at a map? Yeah, exactly like throwing darts. For each dart, it just asks one question using collision checking math. Did I land inside an obstacle or did I land in empty space? Okay, so if the point lands inside a building, it discards it. If the point lands in free space, it saves it as a valid node. Right. It repeats this process rapidly, sprinkling thousands of random nodes across the empty areas of the map.
Once it has enough nodes, it tries to connect them. It looks at neighboring nodes and draws a straight line between them. If that straight line doesn't intersect any obstacles, it creates an edge. It's randomly generating a custom subway map out of thin air. That's a great way to think about it. It builds a mathematical graph dynamically. And once the graph is built, we just use a standard graph search, like Dijkstra's, to find the quickest path through our random web of nodes from start to finish. And because it is only evaluating a sparse network of points rather than a dense continuous grid, PRM is lightning fast, even in massive high -dimensional spaces. It's incredibly fast. But there is a severe probabilistic trade -off. The randomness is a double -edged sword, right? If you have a massive open field, connected to another field by a very narrow, tight doorway.
What are the odds that a random dart happens to land exactly inside that tiny doorway? Statistically very low. This is known in the literature as the narrow passage problem. A PRM planner might fail to find a path entirely, confidently declaring that the goal is unreachable, simply because it failed to sample a node inside the connected corridor. So how do you fix that? Just throw more dart. You can tune the sampling density, sure. Or use advanced heuristics that deliberately cluster samples near the edges of obstacles to force the algorithm to find doorways. But there's another issue. The path generated by a PRM is almost never optimal. Right. Because it's just connecting random dots. The robot will look like it's drunkenly zigzagging between arbitrary points rather than driving a smooth, direct line. Which brings us to the final critical step of navigation.
Whether we use D -Star or PRM, we have generated a path. But we have to loop all the way back to Chapter 4. We have to remember the physics of the wheels. Ah, the non -holonomic constraints again. Exactly. A grid planner or a PRM planner only cares about admissibility. Does this path avoid hitting walls? It does not care at all about feasibility. Can a car actually drive this shape? A D -star grid path might tell the robot to drive 10 meters straight and then instantly turn 90 degrees to the right to go around a box.
A PRM path is full of sharp, jagged angles connecting those random nodes. And if you feed a sharp 90 -degree angle to the pure pursuit controller we discussed earlier on a robot constrained by Ackerman steering, the robot is going to try to turn instantly, but it can't. The physical steering column takes time to rotate. The robot will overshoot the corner. swing wide and potentially crash into the very box the planner was trying to avoid. So the mathematical path must be smooth to respect the kinematic limits of the chassis.
Cork details a few ways to force the math to respect the physics. He starts with Dubin's paths and Reeds–Shepp's paths. I remember those. These algorithms replace jagged angles with geometric curves, right? Yes. They calculate paths using only two shapes, straight lines and circular arcs that perfectly match the maximum turning radius of the specific vehicle. A Dubin's path assumes the car can only drive forward. A Reed's Shep path allows the car to switch gears and drive in reverse, which allows it to calculate complex maneuvers like a three -point turn or a parallel parking sequence. So a Dubin's path is just straight lines connected by sweeping circular arcs. Honestly, that seems perfectly drivable. A car can obviously drive in a circle.
A car can drive a circle, but, and this is a crucial distinction, a car cannot instantaneously transition from a straight line to a circle. Wait, why not? Think about the physics of the steering wheel. When the robot is on the straight segment of the Dubin's path, the steering wheel is dead center. The curvature of the path is zero. The instant the robot hits the beginning of the circular arc, the curvature becomes a fixed non -zero number. Oh. I see. To stay exactly on that path, the steering wheel would have to teleport from the center position to the fully turned position in zero seconds. Which requires infinite actuator velocity. It's physically impossible.
Right, the motor takes time to turn the wheel. Exactly. In reality, the steering motor takes one or two seconds to rotate the wheels. During that time, the robot is still traveling forward, tracing a transition curve that deviates from the perfect Dubin's arc. At high speeds, this discrepancy causes the robot to violently jerk or drift out of its lane. To achieve true high -speed stability, the curvature of the path must be continuous. We need a curve that gradually tightens. And Cork introduces clothoids to solve this, also known as Euler spirals. Yes. A clothoid is a mathematical curve where the curvature changes linearly with the distance traveled along the curve. So as you drive further into a clothoid, the turn gets sharper. at a perfectly steady rate. Exactly. When a robot enters a clothoid, the command to the steering motor is simply a constant velocity. The steering wheel turns smoothly and steadily. Cork points out that roboticists didn't actually invent this. Civil engineers did. This is exactly how highway off -ramps and railway curves are designed. Really? Yep. When you exit a highway at 60 miles an hour, the road doesn't instantly become a tight circle. It is a clothoid spiral. It allows human drivers to turn their steering wheel at a smooth, constant speed to stay perfectly centered in the lane, minimizing lateral jerk. That is deeply satisfying.
Exactly. To see how the software ultimately has to bend to the reality of the hardware. We model the kinematic constraints. We plan a grid path. We smooth it with clothoids so the motors can actually execute it without breaking. The robot starts driving. The plan is perfect. But the world isn't perfect. Exactly. Which brings us to Chapter 6 and what Quark calls the where am I problem? Localization. Because the most beautiful clothoid path in the world is totally useless if the robot's internal map doesn't actually align with reality. Localization is honestly the most pervasive existential crisis in mobile robotics and Quark begins by analyzing the oldest form of navigation used by human sailors for thousands of years. Dead reckoning or odometry.
How does a robot use dead reckoning? It relies entirely on proprioception, its internal sensors. Remember in part one, we talked about integrating sensor data? If a robot knows it started at coordinate zero zero, and its wheel encoders measure that the wheels have rotated enough to move forward at one meter per second for 10 seconds. It just does the basic math and updates its internal belief state. I am now at coordinate 10 zero. Right. It sounds reliable. It's just simple geometry. It is reliable. For about five seconds. Dead reckoning suffers from the accumulation of errors.
Every physical system has noise. Cork divides this into systematic and non -systematic errors. Systematic errors are flaws in the machine itself. For example, your left tire is inflated to 30 psi, and your right tire is inflated to 31 psi. So the right tire has a slightly larger diameter, which means every single time it completes one revolution, it travels a millimeter further than the left tire. Yes. So even though the computer is commanding the robot to drive perfectly straight, The physical chassis is tracing a massive microscopic arc to the left. That's a systematic error. Non -systematic errors come from the environment.
Like the robot drives over a bump, or a tire slips on a patch of dust. Or the carpet pile shifts. The wheel encoder registers rotation, so the computer thinks it moved, but the robot didn't actually move forward. And because dead reckoning works by adding the current movement onto the previous position, these errors don't just exist, they compound. It integrates the error over time. Exactly. If you walk across a room blindfolded, your first step is confident. By your 20th step, you have drifted wildly off course. Roboticists do not represent a robot's position as a single absolute coordinate. They represent it as a probability density function of PDF. OK, so the robot doesn't say, I am at 1010. No. It says, I am most likely near 1010, but I might be at 10 .5, and there's a small chance I'm at 11. And we represent that spreading cloud of uncertainty using a covariance matrix.
Yes. The covariance matrix defines the geometry of our doubt. In a 2D space, if you visualize the covariance matrix, it looks like an ellipse drawn around the robot on the map. The size of the ellipse represents the magnitude of the uncertainty. The orientation of the ellipse, which way it's stretched, is dictated by the off -diagonal elements of the matrix, the cross -covariance. So if a robot is driving straight ahead, but it knows its steering is a bit wobbly, the ellipse stretches out laterally, perpendicular to the robot's motion.
Precisely. As the robot drives blind, relying only on odometry, that covariance ellipse swells. It grows larger and larger, consuming the map, mathematically visualizing the fact that the robot is becoming completely lost. So how do we stop the bleeding? How do we collapse that giant ellipse of uncertainty back down to a tight little dot? We have to open the robot's eyes. We have to look outside the internal sensors and observe the external world. We localize against a landmark map. Let's say the robot has a LIDAR scanner. And it has a digital map that says there is a concrete pillar located at exactly coordinate 20, 20. OK. The LIDAR pings the pillar. It measures the range, say, five meters away. And the bearing, say, 30 degrees to the right. And because the pillar's location on the map is considered absolute truth, the robot can just run the geometry backward.
If that pillar is at 20, 20, and it is five meters away from me, then I must be exactly here. The observation allows the robot to instantly calculate its true global position, overriding the flawed odometry and collapsing the covariance ellipse. But doing this continuously, fusing the noisy drifting odometry with the noisy imperfect LiDAR measurements requires a very specific mathematical engine. It's the centerpiece of Chapter 6, the Extended Cullman Filter. The Kalman filter, originally developed for the Apollo space program, is arguably the most important algorithm in estimation theory.
It is a recursive algorithm that finds the optimal estimate of a system's true state by balancing multiple sources of noisy data. Let's break open the black box. How does the Kalman filter actually negotiate this in practice? It operates in a continuous two -step rhythm. The first step is the predict step. The robot pushes its state forward in time using its physical models and odometry. I commanded the wheels to turn, so I predict I have moved forward one meter. My new estimated pose is X, and my covariance ellipse has grown. That's the dead reckoning part. We predict we moved, and we acknowledge we are more uncertain. What is step two? Step two is the update step. The robot takes a reading from its LIDAR. The Kalman filter does something very clever here. It doesn't just look at the LIDAR data blindly, It uses its predicted position to calculate where it thinks the landmark should be. If I am where I predict I am, the LiDAR should see the pillar at exactly 45 degrees. But the LiDAR actually sees the pillar at, say, 48 degrees.
That three degree difference is the error. We call it the innovation. The innovation is the gap between the robot's internal belief and the external reality. Now, the magic of the Kalman filter happens in a calculation called the Kalman gain. What does the Kalman gain do? The filter has to decide. Do I trust my internal prediction or do I trust the external LiDAR? How does a math equation decide who to trust? It's just numbers. It looks at the variances. It compares the covariance matrix of the robot's prediction against the known noise profile of the LiDAR sensor. If the robot has been driving blind for an hour, its prediction variance is massive.
The Kalman gain shifts heavily toward the sensor. It takes that three degree innovation and forcefully mathematically yanks the robot's estimated position back toward the LIDAR's reality, collapsing the ellipse. But what if it's the other way around? What if the robot just updated its position a millisecond ago? So its odometry's rock solid, but the LIDAR is looking through a cloud of smoke and returning highly noisy data. Then the sensor variance is higher than the prediction variance. The Kalman game drops to near zero. The filter essentially says, I trust my internal math more than this noisy laser reflection. It ignores the observation and keeps trusting the odometry.
It is a brilliant, dynamic statistical balancing act. Predict, update. Predict, update. It continually refines the state estimate. It really is brilliant. But maybe I'm missing something here. I have a massive logic problem with this. What's that? The math assumes the LIDAR knows exactly what it's looking at. The LIDAR is just shooting photons and measuring the time of flight. It just sees a vertical cylinder. In a warehouse, there might be 50 identical concrete pillars. If the Kalman filter thinks it's looking at pillar A, but it's actually looking at pillar B. You have just identified the single most catastrophic failure mode in localization. It is called the data association problem. Yeah, because the LIDAR says, I see a pillar five meters away, but which one? The Kalman filter math relies entirely on matching the observation to the correct coordinate in the map. If the data association is wrong, the consequences are violent. If the robot thinks it is looking at pillar A, which should be two meters away, but is actually looking at pillar B, which is 15 meters away, the calculated innovation, that gap between belief and reality, is going to be massive. And because the LiDAR sensor is generally highly trusted, the Kalman gain is going to take that massive error and apply it to the robot's pose. The math will literally teleport the robot's estimated position 13 meters across the room, shoving it through virtual walls and completely destroying its localization state.
The robot will try to execute path planning from a location it isn't actually in, resulting in a physical crash. So how do we prevent the math from destroying the robot? How do we verify the identity of the pillar? We use statistical gating. Before the Kalman filter is allowed to use an observation, it has to pass a strict confidence test called the Mahalanobis distance. Mahalanobis distance. How is that different from regular Euclidean distance? Euclidean distance is just a ruler. It measures straight line distance. Mahalanobis distance measures how many standard deviations away a point is from a distribution.
It accounts for the shape of the covariance ellipse. When the LiDAR sees a blip, the robot draws a statistical boundary around its predicted location for pillar A based on its current uncertainty. Like a confidence interval. Exactly. If the LIDAR blip falls outside that boundary, meaning the Mahalanobis distance is too large, the robot concludes that the blip is highly unlikely to be Pillar A. It's an anomaly. It could be Pillar B, it could be a person walking by, whatever. Right. And the golden rule of localization is, if you cannot confidently associate the data, you discard it. It is vastly safer to ignore the LiDAR and rely on your drifting odometry for a few more seconds than to feed a mismatched landmark into the Kalman filter and risk a catastrophic pose jump. That makes perfect sense. Better safe than teleporting into a wall. Okay, so we have the Kalman filter. We are managing our uncertainty.
We are gating our data association. We can localize. But there is a glaring assumption we've been making this entire time. We assumed we had a map. Right. We assumed someone handed the robot a pristine, accurate digital blueprint of every pillar and tree in the environment. What if the robot wakes up on the surface of Mars? Or inside a collapsed coal mine? There is no map. Which brings us to the final, most complex topic in mobile robotics. Segment four, SLAM. Simultaneous localization and mapping. The ultimate chicken and egg problem. To understand why SLAM is so incredibly difficult, Cork first defines why pure mapping is easy. If you have a perfect military -grade GPS, your position is known with absolute certainty. You just drive around, your lidar pings a rock, you calculate the geometry, and you write the rock's coordinates into a database.
You are mapping with known poses. Mathematically, Cork notes that the covariance matrix for a pure mapping problem is block diagonal. A block diagonal matrix. Meaning the math for estimating rock A is completely isolated from the math for estimating rock B. The variables don't mix. Exactly. Because your own position is a grounded truth, observing rock A gives you absolutely no new information about rock B. But SLAM removes that grounding truth. You do not have GPS. You are relying on messy odometry and you are trying to build a map of unknown landmarks using sensors mounted on a chassis that doesn't actually know where it is. Cork uses a brilliant historical analogy for this in the book. Captain James Cooke. In 1770, Cook mapped the Eora territory, which is the complex coastline of modern -day Sydney, Australia. Cook was essentially performing slam.
He was on a wooden ship. His odometry was terrible. He was trying to estimate a speed through dynamic ocean currents using a piece of wood tied to a rope. Not exactly a high -precision wheel encoder. No, not at all. His compass headings were noisy. He only had very infrequent, imprecise celestial fixes to anchor his global position. Yet... As he sailed, he had to simultaneously calculate his own trajectory while drawing an accurate map of a totally unknown, jagged coastline based purely on what he could see from the deck. If he thinks he sailed a mile north, but a current actually pushed him a mile east, every single mountain he draws on his map is going to be plotted a mile in the wrong direction. The map is completely corrupted by his positional uncertainty. So how does the Kalman filter solve this? It solves it by expanding the state vector. In basic localization, the Kalman filter is only estimating three numbers, the robots X, Y, and heading. In EKF SLAM, the filter estimates the robot's pose plus the X and Y coordinates of every single landmark it ever sees. The state vector grows continuously as the robot explores. It's trying to balance a massive equation where every single variable is uncertain. And this is where the covariance matrix becomes magical. In SLAM, the covariance matrix is no longer block diagonal. There are no zeros. The matrix is fully correlated. Every single off -diagonal element has a value. Meaning every single object in the map is mathematically tethered to every other object. Think of it like a web of invisible springs. Because the robot's position is uncertain, when it observes landmark A, landmark A's estimated position inherits the robot's uncertainty. Later, it observes landmark B. Landmark B also inherits that same uncertainty.
The errors are shared. The landmarks become deeply correlated with each other through the history of the robot's movement. So let's play this out. Let's say the robot drives in a large circle for 20 minutes. Its dead reckoning is drifting the entire time, so the map it's building is slowly warping. But then... It turns a corner, and its LIDAR sees landmark A again, the very first landmark it mapped. This is called a loop closure, and it is the most important event in SLAM. The robot's dead reckoning says landmark A should be five meters to the left, but the LIDAR sees landmark A five meters to the right.
The robot realizes its odometry has been lying to it for 20 minutes. The Kalman filter calculates a massive innovation. It updates the robot's estimated position, violently pulling it back to reality. But here is the beauty of the fully correlated covariance matrix. When the Kalman filter pulls the robot's pose, the invisible springs in the matrix instantly transmit that tension across the entire map. Because all the landmarks were mapped from that drifting pose, when the pose is corrected, the mathematical springs pull the estimated positions of landmark B, landmark C, and every other landmark the robot has seen, instantly shifting them into a tighter, more accurate configuration. The entire warped map snaps into focus. It's a symphony of statistical correlation. Something. That is so cool. But Cork points out that EKF Slam has a hard ceiling.
It cannot scale. The computational cost is its downfall. Because the matrix is fully correlated every time the robot observes a single landmark, the Kalman filter must update every single element in that massive matrix. If you map 10,000 landmarks in a city, you have a 10,000 by 10,000 matrix. That's 100 million elements. Updating that dense grid of covariance math 30 times a second will melt a standard processor. Furthermore, the extended Kalman filter requires linearization. It uses Taylor series expansions to force nonlinear geometry into linear matrix math. Over time, those approximation errors build up and destroy the map. So the math that got us to the moon isn't enough to map a city. Modern robotics had to evolve. Cork introduces two modern alternatives that bypass the limits of EKF SLAM. The first is PoseGraphSLAM. How does this solve the matrix overload? PoseGraphSLAM abandons the idea of updating a dense, fully correlated matrix in real time. Instead, it turns the robot's trajectory into a mathematical graph, just like the navigation planners we discussed earlier. The nodes in this graph are the robot's historical poses, where it was at time 0, time 1, time 2. The edges connecting those nodes are constraints generated by the sensors. Like, my odometry says pose 2 is exactly 1 meter in front of pose 1. That's an edge. Exactly. Or, my LIDAR saw the same tree from pose 10 and pose 50, so those two poses must be exactly 5 meters apart.
The system is divided into two halves. The front end just casually extracts features and builds this graph of nodes and edges as the robot drives. It doesn't stress about making the map perfect in real time. It just builds the web of constraints. It's just collecting the raw, messy data. Then the back end runs a heavy optimization algorithm, usually nonlinear least squares. It looks at the entire tangled, contradictory graph of springs, and it iteratively shifts all the historical poses around until the tension across all the springs is minimized. Because the connections are sparse, pose 50 is only connected to pose 49 and pose 10, not to pose 2, the math uses sparse matrices, which are incredibly fast to solve. It scales beautifully to massive environments.
It optimizes the entire history of the robot at once. That's so much more efficient. The second modern solution Cork brings up is sequential Monte Carlo, commonly known as particle filters. If pose graph slam is elegant optimization, particle filters sound like brute force computational violence. They are a brilliant application of raw computing power. A Kalman filter tries to elegantly track a single Gaussian ellipse of uncertainty. A particle filter abandons the ellipse entirely. When the robot wakes up and doesn't know where it is, the algorithm scatters thousands, sometimes tens of thousands, of digital particles randomly across the digital map. Every single particle represents a hypothesis. Like, maybe I am this particle in the hallway. Maybe I am this particle in the corner. You basically have a swarm of thousands of virtual ghost robots inside the computer's memory. As the real physical robot drives forward one meter, the algorithm commands every single one of those thousands of ghost particles to also drive forward one meter on the map. But the real robot is driving with noisy wheels, so it injects simulated noise into the ghosts, right? Some ghosts drive 1 .1 meters, some drive 0 .9 meters, some veer slightly left. The swarm naturally spreads out. Then comes the update step. The real robot takes a LiDAR reading and sees a wall exactly 2 meters in front of it. The algorithm then iterates through the swarm. It looks at particle number 1 and asks the map, if the robot is standing exactly where particle number 1 is, how far away should the wall be? If the map says the wall is 10 meters away from particle number 1, but the real LiDAR is seeing the wall at 2 meters, particle number 1 is a terrible hypothesis. So the algorithm assigns it a very low weight. It checks particle number 2. The map says the wall should be 2 .1 meters away. That's a very good hypothesis. It gets a high weight. Once all particles are evaluated, the algorithm performs a resampling step. This is essentially Darwinian evolution applied to statistics. Survival of the fittest data. The algorithm deletes the low -weight particles, the ghosts that were standing in the wrong spots, and it duplicates or clones the high -weight particles. As the robot moves and senses over a few seconds, the particles in the wrong rooms rapidly die off and the particles in the correct room clone and multiply until the entire swarm condenses into a tight, dense cluster precisely over the robot's true location. And this can actually solve the kidnapped robot problem, right?
If I physically pick the robot up and carry it to another room, a Kalman filter will crash because its single ellipse will be thrown into a wall. But a particle filter will just realize all its particles have terrible weights. It will scatter new particles across the map and the swarm will rapidly reconverge on the new location. It is remarkably robust. It handles highly nonlinear sensor noise perfectly because it isn't trying to calculate a Gaussian bell curve. It's just empirically testing reality thousands of times a second. Which brings us to the main takeaway from all of this. If I've learned anything from tracing the path from chapter 4 to chapter 6.
from the bicycle model to the particle swarm. It is that mobile robotics is an endless grueling negotiation. Negotiation is the exact right word. You cannot simply command a physical machine to execute a mathematical ideal. You have to negotiate with the mechanical constraints of Ackerman steering. You have to negotiate with the computational limits of grid planners by using D star or random sampling. You have to negotiate with the physics of inertia using clothoid spiral so you don't flip the robot. And finally, you have to constantly negotiate with the messy shifting reality of the world using Kalman filters and SLAM algorithms to tame the existential uncertainty of localization. SLAM is the ultimate mathematical tether that keeps the digital brain anchored to the physical world. We have successfully navigated the environment. We've mastered moving the entire chassis through space. Next time in Part 3 of 5, Robot Manipulators, we are going to anchor that mobile base to the floor and we are going to dive into the kinematics of moving the robotic arm itself. Moving from the macro -navigation of a warehouse to the micro -precision of a manipulator brings a whole new set of geometric challenges.
But before we sign off, I want to leave you with a final thought to ponder. Throughout this entire deep dive on SLAM and grid planners, we made a massive unspoken assumption. We assumed the obstacles in the environment were static. The walls, the concrete pillars, the trees, they don't move. But what happens to a D -star wavefront or a PRM graph or a SLAM covariance matrix when the obstacles in your map are humans walking through a warehouse, or a fleet of other robots all simultaneously calculating and recalculating their own paths through the same exact space.
How do you mathematically map and navigate a world that refuses to stand still? Keep unpacking that. Thanks for joining us on the Deep Dive.