1 00:00:00,000 --> 00:00:03,200 So back in 2007, there was this robotic vehicle 2 00:00:03,200 --> 00:00:06,139 named Boss that completely made history. It was 3 00:00:06,139 --> 00:00:08,339 built by the Tartan racing team over at Carnegie 4 00:00:08,339 --> 00:00:12,220 Mellon, and it actually won the DARPA Urban Challenge. 5 00:00:12,519 --> 00:00:14,720 Right, which was a massive deal at the time. 6 00:00:14,919 --> 00:00:17,769 Oh. Absolutely huge. It successfully navigated 7 00:00:17,769 --> 00:00:20,530 this really complex, like 60 -mile mock city 8 00:00:20,530 --> 00:00:23,370 course. It was obeying traffic laws, avoiding 9 00:00:23,370 --> 00:00:25,670 other robots, doing all the things you'd expect 10 00:00:25,670 --> 00:00:28,429 a human driver to do. Yeah, and Boss wasn't some 11 00:00:28,429 --> 00:00:31,170 tiny little lab rover either. No, not at all. 12 00:00:31,550 --> 00:00:34,509 It was a heavily modified Chevrolet Tahoe. We're 13 00:00:34,509 --> 00:00:37,710 talking a 5,000 -pound truck with four massive 14 00:00:37,710 --> 00:00:40,869 tires just packed to the brim with lasers, radars, 15 00:00:41,210 --> 00:00:43,490 and computing clusters. But here's the crazy 16 00:00:43,490 --> 00:00:46,759 part. Mathematically speaking, inside its digital 17 00:00:46,759 --> 00:00:50,649 brain, Boss genuinely believed it was riding 18 00:00:50,649 --> 00:00:53,710 a bicycle. Which just sounds so absurd when you 19 00:00:53,710 --> 00:00:56,229 say it out loud. Right? It makes no sense at 20 00:00:56,229 --> 00:00:58,850 first glance, but welcome to the deep dive. We 21 00:00:58,850 --> 00:01:01,369 are so glad you were here with us today. We are 22 00:01:01,369 --> 00:01:03,409 continuing our exploration of the Marginalia 23 00:01:03,409 --> 00:01:06,670 series on Peter Corke's text, Robotics, Vision 24 00:01:06,670 --> 00:01:09,689 and Control, Fundamental Algorithms in Python. 25 00:01:10,109 --> 00:01:13,790 The 2023 edition, just to be precise. Yes, exactly. 26 00:01:13,950 --> 00:01:17,650 This is part two of five, mobile robotics. And 27 00:01:17,650 --> 00:01:19,709 today we're starting with this really strange 28 00:01:19,709 --> 00:01:22,390 paradox. We had to figure out why reducing a 29 00:01:22,390 --> 00:01:25,870 two -ton four -wheeled truck to a mathematical 30 00:01:25,870 --> 00:01:28,390 unicycle or bicycle is basically the only way 31 00:01:28,390 --> 00:01:31,129 to keep it from crashing. It really does seem 32 00:01:31,129 --> 00:01:33,230 completely counterintuitive when you first look 33 00:01:33,230 --> 00:01:36,269 at the engineering of it all. You have this massive 34 00:01:36,269 --> 00:01:39,930 complex mechanical system. And I think your first 35 00:01:39,930 --> 00:01:42,430 instinct as a programmer is to model every single 36 00:01:42,430 --> 00:01:45,120 moving part. Right, you'd want a variable for 37 00:01:45,120 --> 00:01:46,959 the front left wheel, the front right wheel, 38 00:01:47,099 --> 00:01:50,140 the rear suspension. Exactly, the chassis, you 39 00:01:50,140 --> 00:01:52,980 roll, all of it. But Peter Corke introduces this 40 00:01:52,980 --> 00:01:55,700 kinematic bicycle model really early on to demonstrate 41 00:01:55,700 --> 00:01:59,140 a foundational rule of mobile robotics. And that 42 00:01:59,140 --> 00:02:01,400 rule is that mathematical simplification is often 43 00:02:01,400 --> 00:02:04,739 the only path to actual real -time control. You 44 00:02:04,739 --> 00:02:06,980 just have to strip away the physical noise. Yeah, 45 00:02:06,980 --> 00:02:10,020 you strip it away to find the core geometric 46 00:02:10,020 --> 00:02:11,939 truth of how a vehicle actually moves through 47 00:02:11,939 --> 00:02:14,460 space. Now, if you were with us for part one, 48 00:02:14,680 --> 00:02:16,639 remember we spent all our time building those 49 00:02:16,639 --> 00:02:19,039 mathematical foundations. We looked at how to 50 00:02:19,039 --> 00:02:22,259 represent position, orientation, reference frames, 51 00:02:23,080 --> 00:02:25,919 trajectories. The pure geometry of it all. Exactly. 52 00:02:26,259 --> 00:02:28,120 We essentially built the pure geometry of how 53 00:02:28,120 --> 00:02:32,129 things exist and move in abstract space. But, 54 00:02:32,129 --> 00:02:34,349 you know, abstract space doesn't have friction. 55 00:02:34,900 --> 00:02:37,800 Abstract space doesn't have flat tires or dirty 56 00:02:37,800 --> 00:02:41,020 floors or potholes. Unfortunately not. Right. 57 00:02:41,520 --> 00:02:43,560 So today we are taking those abstract reference 58 00:02:43,560 --> 00:02:46,280 frames and we are literally putting them on wheels. 59 00:02:47,250 --> 00:02:49,009 We're covering chapters four, five, and six today. 60 00:02:49,789 --> 00:02:51,870 We have to start by figuring out how a robot 61 00:02:51,870 --> 00:02:54,870 physically interacts with the ground, then how 62 00:02:54,870 --> 00:02:57,789 it plans a path, and finally how it handles the 63 00:02:57,789 --> 00:03:00,610 uncertainty of where it actually is. It's really 64 00:03:00,610 --> 00:03:03,509 a logical progression. Mobility, then navigation, 65 00:03:03,889 --> 00:03:06,330 then localization. So let's jump straight back 66 00:03:06,330 --> 00:03:09,610 to Boss and this bicycle model from chapter four. 67 00:03:10,330 --> 00:03:12,949 Why on earth do we compress a four -wheeled car 68 00:03:12,949 --> 00:03:15,990 into a two -wheeled bike? Well, the answer lies 69 00:03:15,990 --> 00:03:18,789 in how a car actually steers, which is surprisingly 70 00:03:18,789 --> 00:03:21,189 complex when you really break it down. When a 71 00:03:21,189 --> 00:03:23,330 four -wheeled vehicle turns a corner, the two 72 00:03:23,330 --> 00:03:26,409 front wheels cannot point in the exact same direction. 73 00:03:26,830 --> 00:03:29,050 Wait, really? I just assumed they stayed perfectly 74 00:03:29,050 --> 00:03:31,509 parallel. A lot of people assume that. But if 75 00:03:31,509 --> 00:03:34,389 they were perfectly parallel, the car would literally 76 00:03:34,389 --> 00:03:37,449 tear its own tires apart during a turn. Because 77 00:03:37,449 --> 00:03:40,030 the inside wheel is traveling on a tighter, smaller 78 00:03:40,030 --> 00:03:42,430 circle than the outside wheel, right? Yes, exactly. 79 00:03:43,030 --> 00:03:45,250 The inside wheel has to navigate a much shorter 80 00:03:45,250 --> 00:03:47,569 radius, so it has to be turned at a slightly 81 00:03:47,569 --> 00:03:50,370 sharper angle than the outside wheel. If it isn't, 82 00:03:50,569 --> 00:03:52,509 one of those front tires is just going to drag 83 00:03:52,509 --> 00:03:55,530 sideways across the pavement. Ouch. It sounds 84 00:03:55,530 --> 00:03:58,370 terrible for the hardware. It is. And this mechanical 85 00:03:58,370 --> 00:04:00,129 problem was actually solved back in the early 86 00:04:00,129 --> 00:04:02,490 19th century with something called Ackerman steering 87 00:04:02,490 --> 00:04:05,909 geometry. It uses this clever trapezoidal linkage 88 00:04:05,909 --> 00:04:08,409 between the front wheels. So as you turn the 89 00:04:08,409 --> 00:04:10,430 steering wheel, the inner tire naturally angles 90 00:04:10,430 --> 00:04:12,490 more aggressively than the outer tire. Right. 91 00:04:12,889 --> 00:04:15,389 So both front wheels are effectively tracing 92 00:04:15,389 --> 00:04:18,370 concentric circles. They are orbiting a shared 93 00:04:18,370 --> 00:04:21,550 center point. OK, I can visualize that. In robotics, 94 00:04:21,629 --> 00:04:24,149 we call that the instantaneous center of rotation, 95 00:04:24,209 --> 00:04:27,519 or the ICR. Imagine drawing an invisible line 96 00:04:27,519 --> 00:04:29,839 extending straight out from the rear axle of 97 00:04:29,839 --> 00:04:33,399 the car. Okay, got it. Then draw another line 98 00:04:33,399 --> 00:04:36,180 extending out from the front axles, perpendicular 99 00:04:36,180 --> 00:04:38,680 to the angle of the turned wheels. Because of 100 00:04:38,680 --> 00:04:41,019 that Ackerman linkage, the lines from the front 101 00:04:41,019 --> 00:04:43,300 left wheel and the front right wheel will intersect 102 00:04:43,300 --> 00:04:46,180 the rear axle line at the exact same point in 103 00:04:46,180 --> 00:04:49,500 space. And that intersection is the ICR. Exactly. 104 00:04:50,000 --> 00:04:52,160 And as long as the vehicle's moving at low speeds 105 00:04:52,160 --> 00:04:54,540 and the tires aren't slipping, the entire car 106 00:04:54,540 --> 00:04:57,040 is essentially pinned to that invisible point. 107 00:04:57,500 --> 00:04:59,100 Like it's swinging around it like a tetherball? 108 00:04:59,360 --> 00:05:02,139 That's a perfect analogy, yeah. Which means we 109 00:05:02,139 --> 00:05:04,540 don't actually need to model the left and right 110 00:05:04,540 --> 00:05:07,060 wheels independently in our code. Because they 111 00:05:07,060 --> 00:05:09,399 are mathematically locked into orbiting that 112 00:05:09,399 --> 00:05:12,480 same exact ICR, we can just collapse the entire 113 00:05:12,480 --> 00:05:15,220 front axle into a single virtual wheel sitting 114 00:05:15,220 --> 00:05:17,560 right in the dead center of the car. Oh, wow. 115 00:05:17,850 --> 00:05:20,009 And I assume we do the same for the rear axle. 116 00:05:20,189 --> 00:05:23,310 We do. The complex four -wheeled Ackerman geometry 117 00:05:23,310 --> 00:05:25,810 collapses perfectly into a two -wheeled bicycle. 118 00:05:26,250 --> 00:05:29,189 Okay, so the math gets infinitely cleaner. We 119 00:05:29,189 --> 00:05:30,910 can describe the state of this entire vehicle 120 00:05:30,910 --> 00:05:33,129 on a 2D plane using just three numbers, right? 121 00:05:33,430 --> 00:05:36,610 It's x -coordinate, it's y -coordinate, and it's 122 00:05:36,610 --> 00:05:38,569 heading angle. Basically, which direction it's 123 00:05:38,569 --> 00:05:40,910 pointing. Just those three, yep. And we only 124 00:05:40,910 --> 00:05:43,629 have two inputs to worry about. The forward velocity 125 00:05:43,629 --> 00:05:46,110 of the rear wheel and the steering angle of the 126 00:05:46,110 --> 00:05:49,110 front virtual wheel. But, and Cork makes a huge 127 00:05:49,110 --> 00:05:51,649 deal out of this, this simplification exposes 128 00:05:51,649 --> 00:05:54,230 a massive headache. Even though the robot lives 129 00:05:54,230 --> 00:05:56,490 in a 2D world where it can theoretically move 130 00:05:56,490 --> 00:06:00,009 forward, sideways, and rotate, it doesn't actually 131 00:06:00,009 --> 00:06:02,350 have the freedom to do all three. Right. Quirt 132 00:06:02,350 --> 00:06:05,769 calls this a non -holonomic constraint. Non -holonomic 133 00:06:05,769 --> 00:06:07,649 constraint. Yes. Let's unpack that because it 134 00:06:07,649 --> 00:06:10,050 sounds intimidating. It's just a fancy term for 135 00:06:10,050 --> 00:06:12,569 a restriction on the derivative of the robot's 136 00:06:12,569 --> 00:06:14,350 configuration, meaning it's a restriction on 137 00:06:14,350 --> 00:06:17,230 its velocity, but not necessarily a restriction 138 00:06:17,230 --> 00:06:19,970 on its final position. Okay, so for our mathematical 139 00:06:19,970 --> 00:06:23,069 bicycle, what is the actual constraint? The constraint 140 00:06:23,069 --> 00:06:25,350 is simply that the wheels cannot slide laterally. 141 00:06:25,610 --> 00:06:28,170 The velocity along the vehicle's y -axis, sideways, 142 00:06:28,629 --> 00:06:31,569 must always be mathematically zero. I always 143 00:06:31,569 --> 00:06:33,949 picture this like trying to slide a train off 144 00:06:33,949 --> 00:06:38,110 its tracks. The wheels dictate a strict one -dimensional 145 00:06:38,110 --> 00:06:40,949 line of motion at any given microsecond. Yes, 146 00:06:41,029 --> 00:06:43,689 exactly. Like the train can go forward along 147 00:06:43,689 --> 00:06:46,850 the rails or backward along the rails, but it 148 00:06:46,850 --> 00:06:49,709 cannot just effortlessly slide three feet to 149 00:06:49,709 --> 00:06:52,250 the left onto an adjacent track. The physics 150 00:06:52,250 --> 00:06:55,050 of the wheel absolutely forbid it, and this creates 151 00:06:55,050 --> 00:06:58,050 what control theorists call an under -actuated 152 00:06:58,050 --> 00:07:00,939 system. The configuration space has three degrees 153 00:07:00,939 --> 00:07:04,000 of freedom. X, Y, and heading. But you only have 154 00:07:04,000 --> 00:07:07,839 two actuators. The motor driving the wheels and 155 00:07:07,839 --> 00:07:09,439 the motor turning the steering column. Right. 156 00:07:09,899 --> 00:07:12,519 You are missing a lateral actuator. You have 157 00:07:12,519 --> 00:07:15,079 no motor that pushes the car perfectly sideways. 158 00:07:15,839 --> 00:07:18,000 Which is exactly why parallel parking takes so 159 00:07:18,000 --> 00:07:20,540 long for human drivers. If I want my car to move 160 00:07:20,540 --> 00:07:22,300 three feet to the right into a parking space, 161 00:07:22,560 --> 00:07:25,560 I can't just press a move right button. You have 162 00:07:25,560 --> 00:07:28,420 to perform a really complex sequential dance. 163 00:07:28,649 --> 00:07:31,170 Yeah, I pull forward, turn the steering wheel 164 00:07:31,170 --> 00:07:34,250 all the way to the right, reverse, stop, turn 165 00:07:34,250 --> 00:07:36,329 the steering wheel all the way to the left, reverse 166 00:07:36,329 --> 00:07:39,569 again. I am basically trading time and complex 167 00:07:39,569 --> 00:07:42,129 maneuvers to trick an under -actuated system 168 00:07:42,129 --> 00:07:44,850 into achieving a lateral displacement. Exactly. 169 00:07:44,949 --> 00:07:47,129 And you are exploiting the fact that the constraint 170 00:07:47,129 --> 00:07:50,769 is non -holonomic. If it were a holonomic constraint, 171 00:07:51,290 --> 00:07:52,889 it would mean you could literally never reach 172 00:07:52,889 --> 00:07:56,339 that parking spot. Period. Oh, interesting. So 173 00:07:56,339 --> 00:07:59,399 a non -holonomic constraint just means you can't 174 00:07:59,399 --> 00:08:01,980 reach it instantaneously. Right. You have to 175 00:08:01,980 --> 00:08:04,519 integrate your two available controls over time 176 00:08:04,519 --> 00:08:06,839 to arrive at the desired three -dimensional state. 177 00:08:07,620 --> 00:08:09,980 Okay. So how do we actually write code to make 178 00:08:09,980 --> 00:08:13,060 this mathematical bicycle do what we want? Cork 179 00:08:13,060 --> 00:08:15,939 uses a Python simulation package called BDSim, 180 00:08:16,199 --> 00:08:18,839 right? Block diagram simulation. He does. He 181 00:08:18,839 --> 00:08:21,639 uses BDSim to build out four specific controllers. 182 00:08:21,899 --> 00:08:24,060 Let's start with the simplest one, driving to 183 00:08:24,060 --> 00:08:27,149 a point. Let's say we have a robot at coordinate 184 00:08:27,149 --> 00:08:30,149 0, 0, and we want it to go to a target at 10, 185 00:08:30,149 --> 00:08:32,669 10. How does it get there? We use proportional 186 00:08:32,669 --> 00:08:35,330 control. It's a foundational feedback mechanism. 187 00:08:36,110 --> 00:08:38,210 The robot calculates the Euclidean distance to 188 00:08:38,210 --> 00:08:41,289 the target. Let's call that distance rho. It 189 00:08:41,289 --> 00:08:44,269 multiplies rho by a gain factor to set its forward 190 00:08:44,269 --> 00:08:47,649 velocity. So as the robot is far away, rho is 191 00:08:47,649 --> 00:08:50,210 a large number, so it drives fast. Exactly. And 192 00:08:50,210 --> 00:08:53,029 as it approaches the target, rho naturally shrinks 193 00:08:53,029 --> 00:08:55,700 towards 0. and the velocity slows down, bringing 194 00:08:55,700 --> 00:08:58,259 it to a nice, smooth stop. And what about the 195 00:08:58,259 --> 00:09:01,500 steering? Simultaneously, it calculates the angle 196 00:09:01,500 --> 00:09:04,320 to the target and sets its steering angle proportional 197 00:09:04,320 --> 00:09:06,419 to the error between its current heading and 198 00:09:06,419 --> 00:09:08,679 the target heading. Now, there is a very specific, 199 00:09:08,879 --> 00:09:12,100 almost pedantic detail Cork points out when calculating 200 00:09:12,100 --> 00:09:14,220 that angle. You can't just use standard high 201 00:09:14,220 --> 00:09:16,620 school trigonometry. You can't use the standard 202 00:09:16,620 --> 00:09:19,200 inverse tangent function A tan. You have to use 203 00:09:19,200 --> 00:09:23,039 A tan2. Oh, this is such a classic trap for beginners. 204 00:09:23,279 --> 00:09:25,360 Why does a robot care about a slightly different 205 00:09:25,360 --> 00:09:28,440 trig function, it's just an angle? Because standard 206 00:09:28,440 --> 00:09:31,240 ATAN takes a single argument, the ratio of the 207 00:09:31,240 --> 00:09:34,399 y distance divided by the x distance. The mathematical 208 00:09:34,399 --> 00:09:36,820 flaw there is that division destroys information 209 00:09:36,820 --> 00:09:39,360 about the signs of the coordinates. Okay, walk 210 00:09:39,360 --> 00:09:41,240 me through an example of how that breaks the 211 00:09:41,240 --> 00:09:44,480 robot. Sure. Let's say your target is at negative 212 00:09:44,480 --> 00:09:47,460 5 on the x -axis and negative 5 on the y -axis. 213 00:09:47,720 --> 00:09:50,919 You divide negative 5 by negative 5. The negatives 214 00:09:50,919 --> 00:09:54,049 cancel out, leaving a positive 1. OK, basic algebra. 215 00:09:54,490 --> 00:09:56,870 But then the ATAN function looks at that positive 216 00:09:56,870 --> 00:09:59,950 1 and calculates an angle of 45 degrees, which 217 00:09:59,950 --> 00:10:02,370 is in the upper right quadrant. But the target 218 00:10:02,370 --> 00:10:04,529 is actually behind the robot and to the left, 219 00:10:04,710 --> 00:10:08,639 in the lower left quadrant. Exactly. So the robot 220 00:10:08,639 --> 00:10:11,519 would confidently steer in the exact opposite 221 00:10:11,519 --> 00:10:12,960 direction of the goal, thinking it was doing 222 00:10:12,960 --> 00:10:15,539 a great job. That is hilarious. And terrible. 223 00:10:15,840 --> 00:10:18,740 So how does ATAN 2 fix it? The E10 -2 function 224 00:10:18,740 --> 00:10:21,700 solves this by taking the y and x distances as 225 00:10:21,700 --> 00:10:24,240 two separate arguments, rather than dividing 226 00:10:24,240 --> 00:10:26,659 them first. It preserves the negative signs, 227 00:10:26,799 --> 00:10:29,100 checks exactly which of the four quadrants the 228 00:10:29,100 --> 00:10:31,919 target resides in, and returns the unambiguous 229 00:10:31,919 --> 00:10:34,779 globally correct angle. It's a tiny detail in 230 00:10:34,779 --> 00:10:36,860 the Python code, but if you get it wrong, your 231 00:10:36,860 --> 00:10:39,419 robot will relentlessly drive away from its destination. 232 00:10:39,820 --> 00:10:42,820 It's those little translations from pure math 233 00:10:42,820 --> 00:10:45,159 to computer science that always trip you up. 234 00:10:45,309 --> 00:10:48,129 All right, so that's driving to a point. The 235 00:10:48,129 --> 00:10:50,610 second controller, Cork outlines, is driving 236 00:10:50,610 --> 00:10:53,730 along a line. We aren't going to a single coordinate 237 00:10:53,730 --> 00:10:56,809 anymore. We are locking onto an infinite mathematical 238 00:10:56,809 --> 00:10:59,690 line drawn across the plane. And a single proportional 239 00:10:59,690 --> 00:11:02,049 controller won't work here. You need a combined 240 00:11:02,049 --> 00:11:04,289 control law. Why is that? Because if you only 241 00:11:04,289 --> 00:11:06,289 tell the robot to minimize its perpendicular 242 00:11:06,289 --> 00:11:08,789 distance to the line, it will simply turn 90 243 00:11:08,789 --> 00:11:11,250 degrees, drive straight at the line, hit it, 244 00:11:11,350 --> 00:11:14,149 overshoot because it has momentum. realize it's 245 00:11:14,149 --> 00:11:16,809 off the line again, turn around, and wildly oscillate 246 00:11:16,809 --> 00:11:18,549 back and forth across the line. No, it's just 247 00:11:18,549 --> 00:11:20,570 zigzagging forever. So you have to introduce 248 00:11:20,570 --> 00:11:22,889 a second objective. You do. You have to minimize 249 00:11:22,889 --> 00:11:25,450 the distance to the line and simultaneously minimize 250 00:11:25,450 --> 00:11:27,570 the difference between the robot's heading and 251 00:11:27,570 --> 00:11:30,570 the line's angle. Ah, so you steer proportional 252 00:11:30,570 --> 00:11:32,669 to the distance error plus the heading error. 253 00:11:32,929 --> 00:11:36,350 Exactly. The result is this beautiful asymptotic 254 00:11:36,350 --> 00:11:39,570 curve. The robot sweeps gently toward the line 255 00:11:39,570 --> 00:11:42,429 and smooths its steering angle so that by the 256 00:11:42,429 --> 00:11:45,009 time its distance error reaches zero, its heading 257 00:11:45,009 --> 00:11:47,629 error is also exactly zero, and it locks perfectly 258 00:11:47,629 --> 00:11:50,629 onto the track. That makes perfect sense. But 259 00:11:50,629 --> 00:11:52,710 infinite straight lines don't really exist in 260 00:11:52,710 --> 00:11:55,289 a warehouse or a city street, which brings us 261 00:11:55,289 --> 00:11:58,330 to the third controller, driving along a path. 262 00:11:58,809 --> 00:12:02,049 Cork calls this algorithm pure pursuit. Yes, 263 00:12:02,149 --> 00:12:04,259 pure pursuit. You'll see this everywhere in mobile 264 00:12:04,259 --> 00:12:07,080 robotics. So let's say we have a winding arbitrary 265 00:12:07,080 --> 00:12:10,059 path made of hundreds of waypoints. How does 266 00:12:10,059 --> 00:12:13,000 pure pursuit tackle it? Pure pursuit is one of 267 00:12:13,000 --> 00:12:15,299 the most widely used tracking algorithms because 268 00:12:15,299 --> 00:12:18,159 it is just incredibly robust and computationally 269 00:12:18,159 --> 00:12:21,240 cheap. The mechanism is simple. You place a virtual 270 00:12:21,240 --> 00:12:23,500 target point on the path ahead of the robot. 271 00:12:23,610 --> 00:12:26,070 I always picture this as the old carrot and donkey 272 00:12:26,070 --> 00:12:28,909 routine. Yes. The robot is the donkey, the path 273 00:12:28,909 --> 00:12:31,350 is the ground, and we are dangling a virtual 274 00:12:31,350 --> 00:12:33,529 carrot slightly out of reach in front of it. 275 00:12:33,970 --> 00:12:35,870 And the robot just uses that first controller 276 00:12:35,870 --> 00:12:38,759 driving to a point to chase the carrot. And the 277 00:12:38,759 --> 00:12:41,379 crucial variable in that algorithm is the length 278 00:12:41,379 --> 00:12:43,620 of the stick holding the carrot. The distance 279 00:12:43,620 --> 00:12:45,879 from the robot to that virtual target point is 280 00:12:45,879 --> 00:12:48,460 called the lookahead distance. So as the robot 281 00:12:48,460 --> 00:12:51,320 moves forward, the carrot continuously slides 282 00:12:51,320 --> 00:12:53,259 further down the path, always maintaining that 283 00:12:53,259 --> 00:12:56,220 exact lookahead distance. Right. It's constantly 284 00:12:56,220 --> 00:12:58,700 updating. Let's play with that variable, because 285 00:12:58,700 --> 00:13:00,500 I think it highlights the physics perfectly. 286 00:13:01,200 --> 00:13:03,679 What happens to the math if I make the stick 287 00:13:03,679 --> 00:13:07,159 incredibly short? Like, if the carrot is essentially 288 00:13:07,159 --> 00:13:09,360 dangling right in front of the robot's front 289 00:13:09,360 --> 00:13:12,779 bumper. The robot becomes pathologically reactive. 290 00:13:12,960 --> 00:13:15,700 Okay. Because the target point is so close, any 291 00:13:15,700 --> 00:13:18,639 slight curve or tiny deviation in the path causes 292 00:13:18,639 --> 00:13:21,879 a massive, immediate change in the required steering 293 00:13:21,879 --> 00:13:24,220 angle. That's like a student driver staring at 294 00:13:24,220 --> 00:13:26,940 the pavement directly in front of the hood, over 295 00:13:26,940 --> 00:13:30,059 -correcting every half second. Exactly. The robot 296 00:13:30,059 --> 00:13:32,740 will try to execute these violent, high -frequency 297 00:13:32,740 --> 00:13:35,299 steering corrections. The tires will scrub, the 298 00:13:35,299 --> 00:13:37,360 chassis will shake, and if the robot has any 299 00:13:37,360 --> 00:13:40,080 significant mass or speed, those rapid oscillations 300 00:13:40,080 --> 00:13:42,399 will throw it into instability. It might even 301 00:13:42,399 --> 00:13:46,179 flip. Yikes. Okay, so obviously we don't want 302 00:13:46,179 --> 00:13:48,580 a short stick, but what if I lengthen the stick? 303 00:13:48,659 --> 00:13:52,840 What if I push the carrot way out, say, 20 meters 304 00:13:52,840 --> 00:13:56,080 ahead? Well, the steering becomes buttery smooth. 305 00:13:56,399 --> 00:13:58,100 But there's a catch, right? There's a big catch. 306 00:13:58,279 --> 00:14:01,500 It becomes smooth, but it becomes wildly inaccurate. 307 00:14:02,080 --> 00:14:04,700 If the path takes a sharp 90 degree left turn, 308 00:14:04,899 --> 00:14:07,940 but your look ahead distance is 20 meters, the 309 00:14:07,940 --> 00:14:10,279 virtual carrot will round that corner long before 310 00:14:10,279 --> 00:14:13,100 the robot does. Oh, I see. The robot will see 311 00:14:13,100 --> 00:14:15,559 the carrot off to the left, calculate a straight 312 00:14:15,559 --> 00:14:17,899 line proportional response, and simply drive 313 00:14:17,899 --> 00:14:20,139 diagonally right across the grass to get to it. 314 00:14:20,320 --> 00:14:22,100 It completely cuts the corner. It completely 315 00:14:22,100 --> 00:14:25,250 ignores the path. So tuning the look -ahead distance 316 00:14:25,250 --> 00:14:28,289 is this delicate negotiation between tracking 317 00:14:28,289 --> 00:14:31,269 precision and dynamic stability. High -speed 318 00:14:31,269 --> 00:14:33,710 robots absolutely require longer look -ahead 319 00:14:33,710 --> 00:14:36,370 distances to maintain physical stability, which 320 00:14:36,370 --> 00:14:38,690 means their paths must be designed with gentle 321 00:14:38,690 --> 00:14:40,629 sweeping curves so they don't cut those corners. 322 00:14:41,309 --> 00:14:43,370 That physical constraint leads us right into 323 00:14:43,370 --> 00:14:46,509 Cork's fourth controller, driving to a configuration. 324 00:14:47,090 --> 00:14:49,710 This is essentially the parking problem. We don't 325 00:14:49,710 --> 00:14:51,779 just want to arrive at a coordinate. we want 326 00:14:51,779 --> 00:14:53,840 to arrive there with a very specific heading. 327 00:14:54,559 --> 00:14:57,220 And this forces us to confront that non -holonomic 328 00:14:57,220 --> 00:15:00,019 constraint directly. To solve the parking problem 329 00:15:00,019 --> 00:15:02,960 mathematically, Cork performs a really fascinating 330 00:15:02,960 --> 00:15:05,539 transformation in the code. He writes the control 331 00:15:05,539 --> 00:15:08,759 law not for a bicycle, but for a unicycle. A 332 00:15:08,759 --> 00:15:11,840 unicycle. So just a single wheel that can drive 333 00:15:11,840 --> 00:15:14,620 forward and spin in place. Right. A unicycle 334 00:15:14,620 --> 00:15:17,360 model takes two commands, linear velocity and 335 00:15:17,360 --> 00:15:20,580 angular velocity. The math for driving a unicycle 336 00:15:20,580 --> 00:15:23,039 to a specific configuration is well established 337 00:15:23,039 --> 00:15:26,019 and highly elegant. You basically calculate the 338 00:15:26,019 --> 00:15:28,600 required linear speed and the required spin rate 339 00:15:28,600 --> 00:15:31,659 to gracefully spiral into the parking spot. But 340 00:15:31,659 --> 00:15:35,679 our robot isn't a unicycle. It's a bicycle. Boss 341 00:15:35,679 --> 00:15:39,840 is a Tahoe, not a unicycle. True. So we have 342 00:15:39,840 --> 00:15:42,279 to add a final mathematical matrix transformation 343 00:15:42,279 --> 00:15:44,879 at the very end of the loop. We take the desired 344 00:15:44,879 --> 00:15:47,820 angular velocity of the virtual unicycle, and 345 00:15:47,820 --> 00:15:50,159 we use the physical wheelbase length of our bicycle 346 00:15:50,159 --> 00:15:52,980 to calculate the exact front wheel steering angle 347 00:15:52,980 --> 00:15:55,519 required to achieve that same rotation rate. 348 00:15:55,879 --> 00:15:58,580 Wow. We pretend to be a unicycle to figure out 349 00:15:58,580 --> 00:16:01,200 the trajectory, and then we magically translate 350 00:16:01,200 --> 00:16:03,159 that trajectory into steering wheel commands 351 00:16:03,159 --> 00:16:06,179 for the bicycle. It is a brilliant sleight of 352 00:16:06,179 --> 00:16:09,100 hand. It really is. But honestly, as I was reading 353 00:16:09,100 --> 00:16:11,519 this section, watching cork bend over backward 354 00:16:11,519 --> 00:16:14,320 to accommodate this nonholonomic constraint. 355 00:16:14,899 --> 00:16:18,080 I just kept asking myself, why are we fighting 356 00:16:18,080 --> 00:16:21,940 this? What do you mean? Like, if car -like steering 357 00:16:21,940 --> 00:16:25,200 makes the math so incredibly difficult and the 358 00:16:25,200 --> 00:16:28,019 maneuverability so limited, why don't we just 359 00:16:28,019 --> 00:16:30,480 design wheels that can move sideways? Well, you 360 00:16:30,480 --> 00:16:32,620 aren't the first person to ask that. Engineers 361 00:16:32,620 --> 00:16:35,240 have spent decades designing alternative locomotion 362 00:16:35,240 --> 00:16:38,240 to escape the tyranny of Ackerman steering. Cork 363 00:16:38,240 --> 00:16:40,379 actually details two major alternatives in this 364 00:16:40,379 --> 00:16:43,360 chapter. I assume. The first is differential 365 00:16:43,360 --> 00:16:46,259 steering, commonly called skid steering. Think 366 00:16:46,259 --> 00:16:49,919 of a bulldozer, a military tank, or even a standard 367 00:16:49,919 --> 00:16:52,700 Roomba vacuum cleaner. Right. There is no steering 368 00:16:52,700 --> 00:16:55,539 wheel on a Roomba. The wheels on the left side 369 00:16:55,539 --> 00:16:57,000 are locked together, and the wheels on the right 370 00:16:57,000 --> 00:16:59,179 side are locked together. And the control math 371 00:16:59,179 --> 00:17:02,139 for that is blissfully simple. The forward speed 372 00:17:02,139 --> 00:17:04,220 of the robot is just the average speed of the 373 00:17:04,220 --> 00:17:06,680 left and right wheels. The rotation rate of the 374 00:17:06,680 --> 00:17:08,900 robot is proportional to the difference in speed 375 00:17:08,900 --> 00:17:11,200 between the left and right wheels. So if you 376 00:17:11,200 --> 00:17:13,140 drive the left wheels forward at one meter per 377 00:17:13,140 --> 00:17:15,559 second and the right wheels backward at one meter 378 00:17:15,559 --> 00:17:18,240 per second, the vehicle will just spin in place 379 00:17:18,240 --> 00:17:21,099 around its geometric center. Exactly. OK. It 380 00:17:21,099 --> 00:17:23,460 can turn on a dime. It can change its heading 381 00:17:23,460 --> 00:17:26,359 while its linear velocity is strictly zero. That's 382 00:17:26,359 --> 00:17:28,940 amazing. Why don't cars use that? Because the 383 00:17:28,940 --> 00:17:31,579 physical cost is severe. Because the wheels are 384 00:17:31,579 --> 00:17:34,180 locked in a parallel configuration, they physically 385 00:17:34,180 --> 00:17:37,200 resist spinning in place. To rotate the vehicle, 386 00:17:37,440 --> 00:17:39,339 the motors have to generate enough torque to 387 00:17:39,339 --> 00:17:42,200 overcome the lateral friction of the rubber dragging 388 00:17:42,200 --> 00:17:44,839 against the floor. Oh, the tires literally have 389 00:17:44,839 --> 00:17:47,619 to skid sideways. Hence the name skid steering. 390 00:17:48,200 --> 00:17:50,859 And the skidding is highly non -deterministic. 391 00:17:51,359 --> 00:17:53,880 Depending on whether the floor is dusty, wet, 392 00:17:54,039 --> 00:17:56,299 or carpeted, the amount of force required to 393 00:17:56,299 --> 00:17:59,039 skid changes dramatically. Though it's unpredictable. 394 00:17:59,230 --> 00:18:02,470 highly unpredictable. It causes massive wear 395 00:18:02,470 --> 00:18:04,450 on the tires, it drains the battery quickly, 396 00:18:04,690 --> 00:18:07,630 and most critically, it completely destroys your 397 00:18:07,630 --> 00:18:09,730 odometry, which we will see becomes a huge problem 398 00:18:09,730 --> 00:18:13,390 later. You never truly know how far you've turned 399 00:18:13,390 --> 00:18:16,470 because you are constantly slipping. Okay, so 400 00:18:16,470 --> 00:18:18,970 skid steering gives us maneuverability but ruins 401 00:18:18,970 --> 00:18:22,730 our precision. What about a truly holonomic system, 402 00:18:23,069 --> 00:18:25,329 like a wheel that can roll sideways without skidding 403 00:18:25,329 --> 00:18:28,470 at all? Cork introduces meconum wheels here, 404 00:18:28,849 --> 00:18:30,630 which just look like something out of a sci -fi 405 00:18:30,630 --> 00:18:33,490 movie. They really do. They are standard wheels, 406 00:18:33,650 --> 00:18:35,569 but around their circumference they have a series 407 00:18:35,569 --> 00:18:38,549 of passive barrel -shaped rollers mounted at 408 00:18:38,549 --> 00:18:41,930 a strict 45 -degree angle. I've seen these. They 409 00:18:41,930 --> 00:18:45,430 use them on the Kuka U -Bot, right? Yes, the 410 00:18:45,430 --> 00:18:48,210 Kuka U -Bot is famous for them. Mechanum wheels 411 00:18:48,210 --> 00:18:50,890 are an absolute masterpiece of vector mathematics. 412 00:18:51,180 --> 00:18:53,779 When a standard wheel applies torque, the friction 413 00:18:53,779 --> 00:18:56,400 force vector points straight ahead. But because 414 00:18:56,400 --> 00:18:58,859 a mecanum wheel contacts the ground through those 415 00:18:58,859 --> 00:19:01,920 45 degree rollers, the force vector is applied 416 00:19:01,920 --> 00:19:04,940 diagonally. So if you put four of these wheels 417 00:19:04,940 --> 00:19:08,799 on a rectangular chassis, you have four diagonal 418 00:19:08,799 --> 00:19:10,460 force vectors pointing in different directions. 419 00:19:10,880 --> 00:19:13,400 And by independently controlling the speed and 420 00:19:13,400 --> 00:19:15,720 rotation direction of each of those four wheels, 421 00:19:16,099 --> 00:19:19,079 you can add and subtract those vectors. How does 422 00:19:19,079 --> 00:19:21,150 that work in practice? Well, if you want to move 423 00:19:21,150 --> 00:19:23,390 forward, you just spin all four wheels normally. 424 00:19:24,029 --> 00:19:26,349 The lateral sideways components of those diagonal 425 00:19:26,349 --> 00:19:29,150 vectors cancel each other out and the forward 426 00:19:29,150 --> 00:19:31,549 components add together. The robot goes straight. 427 00:19:32,359 --> 00:19:35,160 But if you want a straight sideways, like perfectly 428 00:19:35,160 --> 00:19:38,000 sideways, you drive the wheels on one side toward 429 00:19:38,000 --> 00:19:40,519 each other and the wheels on the other side away 430 00:19:40,519 --> 00:19:42,940 from each other. The forward and backward vectors 431 00:19:42,940 --> 00:19:45,410 completely cancel out. and the lateral vectors 432 00:19:45,410 --> 00:19:48,809 compound. The entire robot glides smoothly to 433 00:19:48,809 --> 00:19:50,930 the right or the left without changing its heading 434 00:19:50,930 --> 00:19:53,529 at all and without skidding a single tire. That 435 00:19:53,529 --> 00:19:57,089 is just incredible. It is a perfectly holonomic 436 00:19:57,089 --> 00:19:59,190 system. You have three degrees of freedom and 437 00:19:59,190 --> 00:20:01,789 you can actuate all of them simultaneously. It 438 00:20:01,789 --> 00:20:05,490 is the holy grail of maneuverability. So I have 439 00:20:05,490 --> 00:20:08,289 to ask again. Why isn't every robot in the world 440 00:20:08,289 --> 00:20:10,809 riding on mech and wheels? Why isn't my Honda 441 00:20:10,809 --> 00:20:13,069 Civic equipped with them to make parallel parking 442 00:20:13,069 --> 00:20:16,039 obsolete? because the physical constraints always 443 00:20:16,039 --> 00:20:19,140 demand payment. Mechanum wheels rely on absolute 444 00:20:19,140 --> 00:20:21,859 precision in how those rollers contact the ground. 445 00:20:22,440 --> 00:20:24,700 The contact patch of a roller is tiny compared 446 00:20:24,700 --> 00:20:27,779 to a standard tire. Furthermore, for the vector 447 00:20:27,779 --> 00:20:29,920 cancellation to work perfectly, all four wheels 448 00:20:29,920 --> 00:20:32,819 must carry an equal load and maintain continuous, 449 00:20:33,440 --> 00:20:36,380 firm friction with the surface. Okay, so what 450 00:20:36,380 --> 00:20:38,759 happens if the robot drives over a small pebble 451 00:20:38,759 --> 00:20:42,329 or one wheel dips into a pothole? The force vector 452 00:20:42,329 --> 00:20:44,750 from that specific wheel disappears. The entire 453 00:20:44,750 --> 00:20:47,029 mathematical balance collapses, and the robot 454 00:20:47,029 --> 00:20:50,920 veers wildly off course. Oh, wow. So they're 455 00:20:50,920 --> 00:20:53,380 incredibly fragile in the real world. Very much 456 00:20:53,380 --> 00:20:57,420 so. Mechanum wheels require pristine, flat, hard 457 00:20:57,420 --> 00:21:00,920 surfaces. They are exceptional for indoor factory 458 00:21:00,920 --> 00:21:03,759 floors, semiconductor fabs, or Amazon fulfillment 459 00:21:03,759 --> 00:21:06,859 centers. But if you take a mechanism wheel outside 460 00:21:06,859 --> 00:21:11,160 on dirt, grass, or uneven asphalt, it is utterly 461 00:21:11,160 --> 00:21:14,039 useless. The roller's jam, the traction fails, 462 00:21:14,140 --> 00:21:16,819 and the math breaks down. And that messy reality, 463 00:21:16,880 --> 00:21:20,160 the physical world, is exactly why the non -holonomic 464 00:21:20,160 --> 00:21:22,539 car -like wheel remains the dominant paradigm 465 00:21:22,539 --> 00:21:26,000 for mobile robotics. It is mathematically annoying, 466 00:21:26,119 --> 00:21:30,259 but it's physically robust. Exactly. You can 467 00:21:30,259 --> 00:21:32,140 drive a Tahoe through the mud, but you can't 468 00:21:32,140 --> 00:21:34,779 drive a U -Bot through the mud. All right, so 469 00:21:34,779 --> 00:21:37,460 we have modeled the physics. We understand the 470 00:21:37,460 --> 00:21:39,799 wheels, the constraints, and the low -level controllers 471 00:21:39,799 --> 00:21:42,460 required to move the chassis. We've officially 472 00:21:42,460 --> 00:21:45,440 conquered Chapter 4, but moving isn't the same 473 00:21:45,440 --> 00:21:48,819 thing as navigating. Not at all. If I put a blindfold 474 00:21:48,819 --> 00:21:50,859 on, I can technically walk, but I don't know 475 00:21:50,859 --> 00:21:53,319 where I'm going. We have to figure out how the 476 00:21:53,319 --> 00:21:55,700 robot decides what path to take through the world, 477 00:21:56,119 --> 00:21:59,680 which brings us to Chapter 5, Navigation. And 478 00:21:59,680 --> 00:22:01,880 Quirk starts this chapter by stripping away almost 479 00:22:01,880 --> 00:22:04,210 all the computing power and showing us navigation 480 00:22:04,210 --> 00:22:07,549 at its most primitive reactive level, Breitenberg 481 00:22:07,549 --> 00:22:11,569 vehicles. Ah, Valentino Breitenberg. He was a 482 00:22:11,569 --> 00:22:14,369 cyberneticist who showed how complex, seemingly 483 00:22:14,369 --> 00:22:16,650 intelligent behavior could arise from incredibly 484 00:22:16,650 --> 00:22:19,789 simple neural wirings. A Breitenberg vehicle 485 00:22:19,789 --> 00:22:22,269 doesn't have a map. It doesn't use coordinate 486 00:22:22,269 --> 00:22:25,029 geometry. It just maps sensor inputs directly 487 00:22:25,029 --> 00:22:28,029 to motor outputs. Cork outlines a vehicle designed 488 00:22:28,029 --> 00:22:30,930 to seek out a light source or maybe a chemical 489 00:22:30,930 --> 00:22:34,039 trail. It has two sensors. One mounted on the 490 00:22:34,039 --> 00:22:37,039 front left, one on the front right. Bilateral 491 00:22:37,039 --> 00:22:39,740 sensing, just like biological eyes. Or the antenna 492 00:22:39,740 --> 00:22:42,900 on a moth. And the wiring is beautifully simple. 493 00:22:43,279 --> 00:22:45,299 You take the signal intensity from the right 494 00:22:45,299 --> 00:22:48,059 sensor, and you feed it directly into the speed 495 00:22:48,059 --> 00:22:50,480 controller for the left wheel. Wait, you cross 496 00:22:50,480 --> 00:22:53,640 the wires? You cross the wires. The cross -wiring 497 00:22:53,640 --> 00:22:56,579 is what creates the behavior. If a light is shining 498 00:22:56,579 --> 00:22:58,839 from the right side of the room, the right sensor 499 00:22:58,839 --> 00:23:01,019 receives a stronger signal than the left sensor. 500 00:23:01,579 --> 00:23:03,880 Because it is wired to the opposite side, the 501 00:23:03,880 --> 00:23:06,339 left wheel receives a command to spin faster 502 00:23:06,339 --> 00:23:08,859 than the right wheel. And if your left wheel 503 00:23:08,859 --> 00:23:11,279 is spinning faster than your right wheel, the 504 00:23:11,279 --> 00:23:14,220 chassis naturally turns to the right. Yes. The 505 00:23:14,220 --> 00:23:16,819 robot automatically arcs toward the light. As 506 00:23:16,819 --> 00:23:19,019 the robot turns and faces the light dead on, 507 00:23:19,259 --> 00:23:21,599 the sensors receive equal signals, the wheels 508 00:23:21,599 --> 00:23:23,980 spin at the exact same speed, and it drives straight 509 00:23:23,980 --> 00:23:26,700 ahead toward the goal. That is brilliant! Now, 510 00:23:26,700 --> 00:23:29,859 the behavior is emergent. Without a single line 511 00:23:29,859 --> 00:23:32,099 of path planning code, without any knowledge 512 00:23:32,099 --> 00:23:35,180 of its XY position in the room, the robot will 513 00:23:35,180 --> 00:23:37,640 gracefully seek out the global maximum of the 514 00:23:37,640 --> 00:23:40,400 light source. It even slows down as it gets closer, 515 00:23:40,539 --> 00:23:42,819 assuming the signal scales appropriately, and 516 00:23:42,819 --> 00:23:45,420 just parks itself directly under the bulb. It 517 00:23:45,420 --> 00:23:49,460 mimics biological tropism perfectly. It's mesmerizing 518 00:23:49,460 --> 00:23:51,779 to watch, because it looks like it's thinking, 519 00:23:52,039 --> 00:23:54,859 like it has intent. But Cork points out that 520 00:23:54,859 --> 00:23:57,240 the illusion breaks the second you introduce 521 00:23:57,240 --> 00:24:00,160 an obstacle. Oh, it fails immediately. If you 522 00:24:00,160 --> 00:24:02,160 place a brick wall right between the Breitenberg 523 00:24:02,160 --> 00:24:04,619 vehicle and the light, it'll just drive straight 524 00:24:04,619 --> 00:24:07,500 into the wall. The sensors are shouting, go forward 525 00:24:07,500 --> 00:24:09,480 to the light, and the motors will grind against 526 00:24:09,480 --> 00:24:11,880 the brick until the batteries die. Because reactive 527 00:24:11,880 --> 00:24:14,799 navigation is fundamentally memoryless. It only 528 00:24:14,799 --> 00:24:17,400 knows the gradient of the signal at the current 529 00:24:17,400 --> 00:24:20,339 instant in time. It cannot formulate a strategy 530 00:24:20,339 --> 00:24:22,619 to go away from the goal temporarily in order 531 00:24:22,619 --> 00:24:25,359 to navigate around a U -shaped obstacle. It's 532 00:24:25,359 --> 00:24:27,819 too simple. To solve a maze, you cannot just 533 00:24:27,819 --> 00:24:30,740 react. You need state. You need memory. You need 534 00:24:30,740 --> 00:24:33,519 a map. Which brings up the granddaddy of map 535 00:24:33,519 --> 00:24:36,579 -based navigation, Shaky the robot, built at 536 00:24:36,579 --> 00:24:39,859 Stanford in the late 1960s. Shaky was the first 537 00:24:39,859 --> 00:24:42,960 robot to actually fuse vision, mapping, and planning. 538 00:24:43,599 --> 00:24:45,940 It would look at a room full of blocks, build 539 00:24:45,940 --> 00:24:48,779 a digital representation of the space, and then 540 00:24:48,779 --> 00:24:51,799 calculate a geometric path to its goal. Corky 541 00:24:51,799 --> 00:24:53,299 explains that there are two fundamental data 542 00:24:53,299 --> 00:24:56,339 structures we use to give a robot a map. Mathematical 543 00:24:56,339 --> 00:24:59,559 graphs and occupancy grids. And they represent 544 00:24:59,559 --> 00:25:02,599 space in two completely different ways. A mathematical 545 00:25:02,599 --> 00:25:05,059 graph in the context of computer science is a 546 00:25:05,059 --> 00:25:07,599 collection of nodes connected by edges. Think 547 00:25:07,599 --> 00:25:11,099 of a highway map or a subway system. So the nodes 548 00:25:11,099 --> 00:25:13,079 are intersections and the edges are the roads 549 00:25:13,079 --> 00:25:16,200 between them. Exactly. Each edge has an associated 550 00:25:16,200 --> 00:25:18,819 cost. maybe the distance or the time it takes 551 00:25:18,819 --> 00:25:21,940 to traverse. Graph search algorithms like Dijkstra's 552 00:25:21,940 --> 00:25:24,900 algorithm or A -star simply explore the network 553 00:25:24,900 --> 00:25:27,019 to find the sequence of edges that results in 554 00:25:27,019 --> 00:25:29,640 the lowest total cost from the start node to 555 00:25:29,640 --> 00:25:32,460 the goal node. Graphs are incredibly efficient, 556 00:25:32,839 --> 00:25:35,019 but they require you to already know the topology 557 00:25:35,019 --> 00:25:37,220 of the space. You have to know where the roads 558 00:25:37,220 --> 00:25:40,059 are beforehand. Right. But if I drop a robot 559 00:25:40,059 --> 00:25:43,099 into an open warehouse or a forest, there are 560 00:25:43,099 --> 00:25:45,640 no defined roads. The robot can theoretically 561 00:25:45,640 --> 00:25:49,200 drive anywhere that isn't a tree or a box. That's 562 00:25:49,200 --> 00:25:52,829 where we need an occupancy grid. Yes. An occupancy 563 00:25:52,829 --> 00:25:56,329 grid discretizes continuous space. You overlay 564 00:25:56,329 --> 00:25:59,269 a giant piece of virtual graph paper across the 565 00:25:59,269 --> 00:26:01,289 environment. You break the world down into tiny 566 00:26:01,289 --> 00:26:04,130 squares, maybe 10 centimeters by 10 centimeters. 567 00:26:04,450 --> 00:26:06,309 And you just assign a binary value to each cell, 568 00:26:06,410 --> 00:26:09,650 right? Yep. It is either free space, meaning 569 00:26:09,650 --> 00:26:12,450 the robot can safely drive there, Or it is occupied, 570 00:26:12,569 --> 00:26:15,329 meaning there is a wall or an obstacle. OK, so 571 00:26:15,329 --> 00:26:17,750 now we have a massive grid of ones and zeros. 572 00:26:18,250 --> 00:26:21,529 How do we actually find a path through it? Quark 573 00:26:21,529 --> 00:26:23,769 explains a technique called the distance transform. 574 00:26:24,069 --> 00:26:26,029 And honestly, this was one of the most elegant 575 00:26:26,029 --> 00:26:28,690 concepts in the entire chapter. It really is 576 00:26:28,690 --> 00:26:31,809 beautiful math. The distance transform is a way 577 00:26:31,809 --> 00:26:34,630 front expansion algorithm. You start by identifying 578 00:26:34,630 --> 00:26:36,910 the specific cell in your grid that represents 579 00:26:36,910 --> 00:26:39,869 your goal destination. You assign that cell a 580 00:26:39,869 --> 00:26:42,779 value of zero. OK. Goal is zero. Then you look 581 00:26:42,779 --> 00:26:44,960 at all the immediate neighbor cells, the ones 582 00:26:44,960 --> 00:26:47,539 adjacent to the goal. Assuming they are free 583 00:26:47,539 --> 00:26:50,440 space, you assign them a value of one. So you 584 00:26:50,440 --> 00:26:52,579 are basically measuring the distance to the goal 585 00:26:52,579 --> 00:26:56,039 in units of grid cells. Exactly. Then you look 586 00:26:56,039 --> 00:26:58,359 at the unvisited neighbors of those cells and 587 00:26:58,359 --> 00:27:01,359 assign them a two. You iterate this process over 588 00:27:01,359 --> 00:27:04,380 and over. It's mathematically identical to dropping 589 00:27:04,380 --> 00:27:07,720 a pebble into a still pond. A way front of numbers 590 00:27:07,720 --> 00:27:10,609 ripples outward from the goal. And when the wayfront 591 00:27:10,609 --> 00:27:13,349 hits an obstacle cell. It can't pass through, 592 00:27:13,710 --> 00:27:15,750 so it flows around the edges of the obstacle, 593 00:27:15,970 --> 00:27:18,190 filling every single pocket of free space in 594 00:27:18,190 --> 00:27:20,910 the map, until the wayfront finally washes over 595 00:27:20,910 --> 00:27:23,349 the specific cell where the robot is currently 596 00:27:23,349 --> 00:27:25,750 sitting. And once that grid is populated with 597 00:27:25,750 --> 00:27:28,329 those rippling numbers, the planning is trivial, 598 00:27:28,349 --> 00:27:30,849 right? The robot doesn't even have to think. 599 00:27:30,950 --> 00:27:33,029 It does no thinking at all. No. It just looks 600 00:27:33,029 --> 00:27:35,109 at the eight cells immediately surrounding it, 601 00:27:35,230 --> 00:27:37,309 finds the cell with the lowest number, and moves 602 00:27:37,309 --> 00:27:40,519 there. It just rolls. downhill along the gradient, 603 00:27:40,940 --> 00:27:43,980 50, 49, 48, flowing around corners taking the 604 00:27:43,980 --> 00:27:46,759 absolute shortest path straight to the zero at 605 00:27:46,759 --> 00:27:48,740 the goal. It is mathematically guaranteed to 606 00:27:48,740 --> 00:27:51,779 find the shortest path if one exists. But, there's 607 00:27:51,779 --> 00:27:55,519 always a but. It assumes a binary world. It assumes 608 00:27:55,519 --> 00:27:59,319 all free space is equally desirable. In reality, 609 00:27:59,859 --> 00:28:01,920 a warehouse might have a smooth concrete aisle 610 00:28:01,920 --> 00:28:05,240 and a cluttered bumpy storage area. Both are 611 00:28:05,240 --> 00:28:08,680 technically free space. But the bumpy area requires 612 00:28:08,680 --> 00:28:11,839 way more energy and time to traverse. We need 613 00:28:11,839 --> 00:28:13,900 a way to encode the difficulty of the terrain. 614 00:28:14,220 --> 00:28:17,720 Which introduces D -star, or dynamic A -star. 615 00:28:17,940 --> 00:28:20,000 Okay. This is an essential upgrade to the basic 616 00:28:20,000 --> 00:28:22,400 grid planner. D -star transitions from a simple 617 00:28:22,400 --> 00:28:25,759 distance metric to a traversability cost. We 618 00:28:25,759 --> 00:28:28,160 assign a weight to every single cell. Okay, give 619 00:28:28,160 --> 00:28:30,299 me an example of how that changes the math. Sure. 620 00:28:30,579 --> 00:28:33,160 The smooth concrete might cost one unit of energy 621 00:28:33,160 --> 00:28:36,230 to cross. The bumpy storage area might cost five 622 00:28:36,230 --> 00:28:39,109 units. The algorithm calculates the wavefront 623 00:28:39,109 --> 00:28:42,009 by accumulating these costs. So the robot might 624 00:28:42,009 --> 00:28:44,569 actively choose a path that is physically longer, 625 00:28:44,970 --> 00:28:47,769 in actual distance, but mathematically cheaper 626 00:28:47,769 --> 00:28:50,509 in energy because it goes around the high cost 627 00:28:50,509 --> 00:28:53,349 terrain. That is really smart. But the real power 628 00:28:53,349 --> 00:28:56,789 of D -Star is the dynamic part, right? A standard 629 00:28:56,789 --> 00:28:59,329 distance transform is incredibly brittle. If 630 00:28:59,329 --> 00:29:02,250 the robot calculates its beautiful optimal path, 631 00:29:02,430 --> 00:29:04,990 starts driving, and suddenly its lidar detects 632 00:29:04,990 --> 00:29:07,349 that someone left a forklift parked right in 633 00:29:07,349 --> 00:29:09,690 the middle of the aisle. The path is blocked. 634 00:29:10,309 --> 00:29:13,029 Right. And in a naive algorithm, discovering 635 00:29:13,029 --> 00:29:15,230 a new obstacle means you have to throw away the 636 00:29:15,230 --> 00:29:18,369 entire map, reassign the forklift cells as occupied, 637 00:29:18,769 --> 00:29:20,869 and run the entire wavefront expansion again 638 00:29:20,869 --> 00:29:23,450 from scratch. Which takes forever. If your grid 639 00:29:23,450 --> 00:29:25,670 has a million cells, doing that recalculation 640 00:29:25,670 --> 00:29:28,589 every time you see a new box is computationally 641 00:29:28,589 --> 00:29:31,029 ruinous. The robot would just freeze in place 642 00:29:31,029 --> 00:29:33,170 for five seconds every time a person walked by. 643 00:29:33,650 --> 00:29:36,869 So how does D -Star avoid starting over? Under 644 00:29:36,869 --> 00:29:39,150 the hood, as D -Star builds the initial path, 645 00:29:39,349 --> 00:29:41,849 it maintains a network of back pointers. Every 646 00:29:41,849 --> 00:29:44,009 cell remembers which neighbor it received its 647 00:29:44,009 --> 00:29:47,730 cost from. That's clever. It is. When an unexpected 648 00:29:47,730 --> 00:29:50,150 obstacle appears, the robot just updates the 649 00:29:50,150 --> 00:29:52,789 cost of those specific blocked cells to infinity. 650 00:29:53,369 --> 00:29:55,930 Then it uses those back pointers to trace the 651 00:29:55,930 --> 00:29:58,809 shock wave of that change outward, only recalculating 652 00:29:58,809 --> 00:30:00,769 the costs of the local cells that are directly 653 00:30:00,769 --> 00:30:03,069 affected by the blockage. So the vast majority 654 00:30:03,069 --> 00:30:05,769 of the map remains completely untouched. Exactly. 655 00:30:06,349 --> 00:30:08,470 D -STAR incrementally repairs the path on the 656 00:30:08,470 --> 00:30:11,049 fly, making it incredibly efficient for real 657 00:30:11,049 --> 00:30:14,210 -world changing navigation. It's an elegant solution 658 00:30:14,210 --> 00:30:17,279 for a 2D warehouse floor. But I kept thinking 659 00:30:17,279 --> 00:30:20,200 about scale while reading this. If I want a robot 660 00:30:20,200 --> 00:30:22,700 to navigate across an entire university campus, 661 00:30:23,140 --> 00:30:25,140 and I'm using a 10 -centimeter grid to capture 662 00:30:25,140 --> 00:30:28,140 the detail of the doorways and curbs, that matrix 663 00:30:28,140 --> 00:30:30,599 is going to contain hundreds of millions of cells. 664 00:30:31,000 --> 00:30:33,059 Even D star is going to choke on that much memory. 665 00:30:33,599 --> 00:30:35,880 You are hitting the curse of dimensionality. 666 00:30:36,279 --> 00:30:39,619 Grid -based planners scale terribly. The computational 667 00:30:39,619 --> 00:30:42,039 cost grows exponentially with the size of the 668 00:30:42,039 --> 00:30:44,470 environment and the resolution of the grid. If 669 00:30:44,470 --> 00:30:47,690 you move from a 2D floor plan to 3D space, say 670 00:30:47,690 --> 00:30:49,569 path planning for a drone flying through a city, 671 00:30:50,170 --> 00:30:52,569 a volumetric grid becomes instantly impossible 672 00:30:52,569 --> 00:30:55,309 to compute in real time. So we have to abandon 673 00:30:55,309 --> 00:30:58,789 the grid entirely. We do. Cork introduces a completely 674 00:30:58,789 --> 00:31:01,430 different approach to path planning called PRM, 675 00:31:01,829 --> 00:31:04,470 probabilistic roadmaps. And the philosophy here 676 00:31:04,470 --> 00:31:06,250 is that we shouldn't try to map every single 677 00:31:06,250 --> 00:31:08,789 inch of the world. We should just sample it. 678 00:31:09,069 --> 00:31:12,250 Just sample it. How does that work? PRM relies 679 00:31:12,250 --> 00:31:14,670 on random sampling within the configuration space. 680 00:31:15,369 --> 00:31:17,410 Instead of dividing the map into millions of 681 00:31:17,410 --> 00:31:20,009 tiny squares and checking every single one, the 682 00:31:20,009 --> 00:31:23,549 algorithm just generates random arbitrary coordinates. 683 00:31:24,109 --> 00:31:26,329 Like throwing darts at a map? Yeah, exactly like 684 00:31:26,329 --> 00:31:29,410 throwing darts. For each dart, it just asks one 685 00:31:29,410 --> 00:31:31,849 question using collision checking math. Did I 686 00:31:31,849 --> 00:31:34,710 land inside an obstacle or did I land in empty 687 00:31:34,710 --> 00:31:38,170 space? Okay, so if the point lands inside a building, 688 00:31:38,509 --> 00:31:41,049 it discards it. If the point lands in free space, 689 00:31:41,250 --> 00:31:44,329 it saves it as a valid node. Right. It repeats 690 00:31:44,329 --> 00:31:46,829 this process rapidly, sprinkling thousands of 691 00:31:46,829 --> 00:31:48,910 random nodes across the empty areas of the map. 692 00:31:49,390 --> 00:31:51,569 Once it has enough nodes, it tries to connect 693 00:31:51,569 --> 00:31:53,809 them. It looks at neighboring nodes and draws 694 00:31:53,809 --> 00:31:56,089 a straight line between them. If that straight 695 00:31:56,089 --> 00:31:58,609 line doesn't intersect any obstacles, it creates 696 00:31:58,609 --> 00:32:01,430 an edge. It's randomly generating a custom subway 697 00:32:01,430 --> 00:32:03,430 map out of thin air. That's a great way to think 698 00:32:03,430 --> 00:32:06,369 about it. It builds a mathematical graph dynamically. 699 00:32:06,589 --> 00:32:08,990 And once the graph is built, we just use a standard 700 00:32:08,990 --> 00:32:12,029 graph search, like Dijkstra's, to find the quickest 701 00:32:12,029 --> 00:32:14,650 path through our random web of nodes from start 702 00:32:14,650 --> 00:32:16,750 to finish. And because it is only evaluating 703 00:32:16,750 --> 00:32:19,730 a sparse network of points rather than a dense 704 00:32:19,730 --> 00:32:23,750 continuous grid, PRM is lightning fast, even 705 00:32:23,750 --> 00:32:26,769 in massive high -dimensional spaces. It's incredibly 706 00:32:26,769 --> 00:32:30,009 fast. But there is a severe probabilistic trade 707 00:32:30,009 --> 00:32:32,710 -off. The randomness is a double -edged sword, 708 00:32:33,230 --> 00:32:35,859 right? If you have a massive open field, connected 709 00:32:35,859 --> 00:32:38,319 to another field by a very narrow, tight doorway. 710 00:32:39,500 --> 00:32:41,880 What are the odds that a random dart happens 711 00:32:41,880 --> 00:32:45,660 to land exactly inside that tiny doorway? Statistically 712 00:32:45,660 --> 00:32:48,279 very low. This is known in the literature as 713 00:32:48,279 --> 00:32:51,319 the narrow passage problem. A PRM planner might 714 00:32:51,319 --> 00:32:54,500 fail to find a path entirely, confidently declaring 715 00:32:54,500 --> 00:32:56,660 that the goal is unreachable, simply because 716 00:32:56,660 --> 00:32:58,779 it failed to sample a node inside the connected 717 00:32:58,779 --> 00:33:01,559 corridor. So how do you fix that? Just throw 718 00:33:01,559 --> 00:33:04,170 more dart. You can tune the sampling density, 719 00:33:04,549 --> 00:33:07,809 sure. Or use advanced heuristics that deliberately 720 00:33:07,809 --> 00:33:10,190 cluster samples near the edges of obstacles to 721 00:33:10,190 --> 00:33:12,930 force the algorithm to find doorways. But there's 722 00:33:12,930 --> 00:33:15,710 another issue. The path generated by a PRM is 723 00:33:15,710 --> 00:33:18,430 almost never optimal. Right. Because it's just 724 00:33:18,430 --> 00:33:21,240 connecting random dots. The robot will look like 725 00:33:21,240 --> 00:33:24,579 it's drunkenly zigzagging between arbitrary points 726 00:33:24,579 --> 00:33:27,849 rather than driving a smooth, direct line. Which 727 00:33:27,849 --> 00:33:30,569 brings us to the final critical step of navigation. 728 00:33:30,910 --> 00:33:33,990 Whether we use D -Star or PRM, we have generated 729 00:33:33,990 --> 00:33:36,430 a path. But we have to loop all the way back 730 00:33:36,430 --> 00:33:39,410 to Chapter 4. We have to remember the physics 731 00:33:39,410 --> 00:33:42,809 of the wheels. Ah, the non -holonomic constraints 732 00:33:42,809 --> 00:33:46,769 again. Exactly. A grid planner or a PRM planner 733 00:33:46,769 --> 00:33:49,650 only cares about admissibility. Does this path 734 00:33:49,650 --> 00:33:52,089 avoid hitting walls? It does not care at all 735 00:33:52,089 --> 00:33:55,650 about feasibility. Can a car actually drive this 736 00:33:55,650 --> 00:33:58,480 shape? A D -star grid path might tell the robot 737 00:33:58,480 --> 00:34:01,059 to drive 10 meters straight and then instantly 738 00:34:01,059 --> 00:34:03,400 turn 90 degrees to the right to go around a box. 739 00:34:04,240 --> 00:34:07,339 A PRM path is full of sharp, jagged angles connecting 740 00:34:07,339 --> 00:34:10,420 those random nodes. And if you feed a sharp 90 741 00:34:10,420 --> 00:34:12,300 -degree angle to the pure pursuit controller 742 00:34:12,300 --> 00:34:15,159 we discussed earlier on a robot constrained by 743 00:34:15,159 --> 00:34:17,559 Ackerman steering, the robot is going to try 744 00:34:17,559 --> 00:34:20,320 to turn instantly, but it can't. The physical 745 00:34:20,320 --> 00:34:23,099 steering column takes time to rotate. The robot 746 00:34:23,099 --> 00:34:26,079 will overshoot the corner. swing wide and potentially 747 00:34:26,079 --> 00:34:28,599 crash into the very box the planner was trying 748 00:34:28,599 --> 00:34:31,820 to avoid. So the mathematical path must be smooth 749 00:34:31,820 --> 00:34:35,380 to respect the kinematic limits of the chassis. 750 00:34:36,260 --> 00:34:38,500 Cork details a few ways to force the math to 751 00:34:38,500 --> 00:34:41,699 respect the physics. He starts with Dubin's paths 752 00:34:41,699 --> 00:34:44,760 and Reeds–Shepp's paths. I remember those. These 753 00:34:44,760 --> 00:34:47,340 algorithms replace jagged angles with geometric 754 00:34:47,340 --> 00:34:50,030 curves, right? Yes. They calculate paths using 755 00:34:50,030 --> 00:34:52,690 only two shapes, straight lines and circular 756 00:34:52,690 --> 00:34:55,070 arcs that perfectly match the maximum turning 757 00:34:55,070 --> 00:34:58,289 radius of the specific vehicle. A Dubin's path 758 00:34:58,289 --> 00:35:01,670 assumes the car can only drive forward. A Reed's 759 00:35:01,670 --> 00:35:03,969 Shep path allows the car to switch gears and 760 00:35:03,969 --> 00:35:06,349 drive in reverse, which allows it to calculate 761 00:35:06,349 --> 00:35:08,969 complex maneuvers like a three -point turn or 762 00:35:08,969 --> 00:35:12,750 a parallel parking sequence. So a Dubin's path 763 00:35:12,750 --> 00:35:14,570 is just straight lines connected by sweeping 764 00:35:14,570 --> 00:35:17,469 circular arcs. Honestly, that seems perfectly 765 00:35:17,469 --> 00:35:19,809 drivable. A car can obviously drive in a circle. 766 00:35:20,289 --> 00:35:22,849 A car can drive a circle, but, and this is a 767 00:35:22,849 --> 00:35:26,070 crucial distinction, a car cannot instantaneously 768 00:35:26,070 --> 00:35:27,929 transition from a straight line to a circle. 769 00:35:28,570 --> 00:35:30,929 Wait, why not? Think about the physics of the 770 00:35:30,929 --> 00:35:33,769 steering wheel. When the robot is on the straight 771 00:35:33,769 --> 00:35:36,130 segment of the Dubin's path, the steering wheel 772 00:35:36,130 --> 00:35:38,929 is dead center. The curvature of the path is 773 00:35:38,929 --> 00:35:41,730 zero. The instant the robot hits the beginning 774 00:35:41,730 --> 00:35:44,190 of the circular arc, the curvature becomes a 775 00:35:44,190 --> 00:35:48,610 fixed non -zero number. Oh. I see. To stay exactly 776 00:35:48,610 --> 00:35:50,550 on that path, the steering wheel would have to 777 00:35:50,550 --> 00:35:53,190 teleport from the center position to the fully 778 00:35:53,190 --> 00:35:55,690 turned position in zero seconds. Which requires 779 00:35:55,690 --> 00:35:59,070 infinite actuator velocity. It's physically impossible. 780 00:35:59,750 --> 00:36:01,969 Right, the motor takes time to turn the wheel. 781 00:36:02,170 --> 00:36:04,769 Exactly. In reality, the steering motor takes 782 00:36:04,769 --> 00:36:07,670 one or two seconds to rotate the wheels. During 783 00:36:07,670 --> 00:36:09,869 that time, the robot is still traveling forward, 784 00:36:10,050 --> 00:36:13,210 tracing a transition curve that deviates from 785 00:36:13,210 --> 00:36:16,510 the perfect Dubin's arc. At high speeds, this 786 00:36:16,510 --> 00:36:19,110 discrepancy causes the robot to violently jerk 787 00:36:19,110 --> 00:36:22,429 or drift out of its lane. To achieve true high 788 00:36:22,429 --> 00:36:25,789 -speed stability, the curvature of the path must 789 00:36:25,789 --> 00:36:28,429 be continuous. We need a curve that gradually 790 00:36:28,429 --> 00:36:31,889 tightens. And Cork introduces clothoids to solve 791 00:36:31,889 --> 00:36:35,090 this, also known as Euler spirals. Yes. A clothoid 792 00:36:35,090 --> 00:36:37,789 is a mathematical curve where the curvature changes 793 00:36:37,789 --> 00:36:40,530 linearly with the distance traveled along the 794 00:36:40,530 --> 00:36:43,429 curve. So as you drive further into a clothoid, 795 00:36:43,469 --> 00:36:46,099 the turn gets sharper. at a perfectly steady 796 00:36:46,099 --> 00:36:49,480 rate. Exactly. When a robot enters a clothoid, 797 00:36:49,719 --> 00:36:51,820 the command to the steering motor is simply a 798 00:36:51,820 --> 00:36:55,079 constant velocity. The steering wheel turns smoothly 799 00:36:55,079 --> 00:36:58,059 and steadily. Cork points out that roboticists 800 00:36:58,059 --> 00:37:00,840 didn't actually invent this. Civil engineers 801 00:37:00,840 --> 00:37:03,900 did. This is exactly how highway off -ramps and 802 00:37:03,900 --> 00:37:06,539 railway curves are designed. Really? Yep. When 803 00:37:06,539 --> 00:37:09,519 you exit a highway at 60 miles an hour, the road 804 00:37:09,519 --> 00:37:12,119 doesn't instantly become a tight circle. It is 805 00:37:12,119 --> 00:37:14,920 a clothoid spiral. It allows human drivers to 806 00:37:14,920 --> 00:37:17,119 turn their steering wheel at a smooth, constant 807 00:37:17,119 --> 00:37:19,360 speed to stay perfectly centered in the lane, 808 00:37:20,000 --> 00:37:23,079 minimizing lateral jerk. That is deeply satisfying. 809 00:37:23,119 --> 00:37:25,300 Exactly. To see how the software ultimately has 810 00:37:25,300 --> 00:37:28,599 to bend to the reality of the hardware. We model 811 00:37:28,599 --> 00:37:31,719 the kinematic constraints. We plan a grid path. 812 00:37:32,260 --> 00:37:34,980 We smooth it with clothoids so the motors can 813 00:37:34,980 --> 00:37:37,440 actually execute it without breaking. The robot 814 00:37:37,440 --> 00:37:40,989 starts driving. The plan is perfect. But the 815 00:37:40,989 --> 00:37:43,369 world isn't perfect. Exactly. Which brings us 816 00:37:43,369 --> 00:37:46,489 to Chapter 6 and what Quark calls the where am 817 00:37:46,489 --> 00:37:50,260 I problem? Localization. Because the most beautiful 818 00:37:50,260 --> 00:37:52,340 clothoid path in the world is totally useless 819 00:37:52,340 --> 00:37:54,559 if the robot's internal map doesn't actually 820 00:37:54,559 --> 00:37:58,119 align with reality. Localization is honestly 821 00:37:58,119 --> 00:38:01,320 the most pervasive existential crisis in mobile 822 00:38:01,320 --> 00:38:04,460 robotics and Quark begins by analyzing the oldest 823 00:38:04,460 --> 00:38:06,920 form of navigation used by human sailors for 824 00:38:06,920 --> 00:38:09,840 thousands of years. Dead reckoning or odometry. 825 00:38:10,719 --> 00:38:13,909 How does a robot use dead reckoning? It relies 826 00:38:13,909 --> 00:38:17,070 entirely on proprioception, its internal sensors. 827 00:38:17,849 --> 00:38:19,690 Remember in part one, we talked about integrating 828 00:38:19,690 --> 00:38:23,170 sensor data? If a robot knows it started at coordinate 829 00:38:23,170 --> 00:38:25,949 zero zero, and its wheel encoders measure that 830 00:38:25,949 --> 00:38:27,889 the wheels have rotated enough to move forward 831 00:38:27,889 --> 00:38:31,369 at one meter per second for 10 seconds. It just 832 00:38:31,369 --> 00:38:33,630 does the basic math and updates its internal 833 00:38:33,630 --> 00:38:36,570 belief state. I am now at coordinate 10 zero. 834 00:38:36,949 --> 00:38:39,949 Right. It sounds reliable. It's just simple geometry. 835 00:38:40,329 --> 00:38:43,380 It is reliable. For about five seconds. Dead 836 00:38:43,380 --> 00:38:45,619 reckoning suffers from the accumulation of errors. 837 00:38:46,000 --> 00:38:48,639 Every physical system has noise. Cork divides 838 00:38:48,639 --> 00:38:51,139 this into systematic and non -systematic errors. 839 00:38:51,480 --> 00:38:53,420 Systematic errors are flaws in the machine itself. 840 00:38:53,639 --> 00:38:56,039 For example, your left tire is inflated to 30 841 00:38:56,039 --> 00:38:59,340 psi, and your right tire is inflated to 31 psi. 842 00:38:59,719 --> 00:39:01,900 So the right tire has a slightly larger diameter, 843 00:39:02,300 --> 00:39:04,239 which means every single time it completes one 844 00:39:04,239 --> 00:39:06,920 revolution, it travels a millimeter further than 845 00:39:06,920 --> 00:39:09,940 the left tire. Yes. So even though the computer 846 00:39:09,940 --> 00:39:12,219 is commanding the robot to drive perfectly straight, 847 00:39:12,349 --> 00:39:15,929 The physical chassis is tracing a massive microscopic 848 00:39:15,929 --> 00:39:19,510 arc to the left. That's a systematic error. Non 849 00:39:19,510 --> 00:39:21,369 -systematic errors come from the environment. 850 00:39:21,929 --> 00:39:24,190 Like the robot drives over a bump, or a tire 851 00:39:24,190 --> 00:39:26,329 slips on a patch of dust. Or the carpet pile 852 00:39:26,329 --> 00:39:28,949 shifts. The wheel encoder registers rotation, 853 00:39:29,030 --> 00:39:31,110 so the computer thinks it moved, but the robot 854 00:39:31,110 --> 00:39:33,510 didn't actually move forward. And because dead 855 00:39:33,510 --> 00:39:35,449 reckoning works by adding the current movement 856 00:39:35,449 --> 00:39:37,869 onto the previous position, these errors don't 857 00:39:37,869 --> 00:39:40,670 just exist, they compound. It integrates the 858 00:39:40,670 --> 00:39:43,599 error over time. Exactly. If you walk across 859 00:39:43,599 --> 00:39:46,099 a room blindfolded, your first step is confident. 860 00:39:46,380 --> 00:39:49,360 By your 20th step, you have drifted wildly off 861 00:39:49,360 --> 00:39:52,679 course. Roboticists do not represent a robot's 862 00:39:52,679 --> 00:39:55,420 position as a single absolute coordinate. They 863 00:39:55,420 --> 00:39:58,539 represent it as a probability density function 864 00:39:58,539 --> 00:40:02,820 of PDF. OK, so the robot doesn't say, I am at 865 00:40:02,820 --> 00:40:07,599 1010. No. It says, I am most likely near 1010, 866 00:40:07,619 --> 00:40:10,239 but I might be at 10 .5, and there's a small 867 00:40:10,239 --> 00:40:13,139 chance I'm at 11. And we represent that spreading 868 00:40:13,139 --> 00:40:16,380 cloud of uncertainty using a covariance matrix. 869 00:40:16,840 --> 00:40:20,199 Yes. The covariance matrix defines the geometry 870 00:40:20,199 --> 00:40:23,360 of our doubt. In a 2D space, if you visualize 871 00:40:23,360 --> 00:40:25,639 the covariance matrix, it looks like an ellipse 872 00:40:25,639 --> 00:40:28,179 drawn around the robot on the map. The size of 873 00:40:28,179 --> 00:40:30,599 the ellipse represents the magnitude of the uncertainty. 874 00:40:31,000 --> 00:40:33,599 The orientation of the ellipse, which way it's 875 00:40:33,599 --> 00:40:36,659 stretched, is dictated by the off -diagonal elements 876 00:40:36,659 --> 00:40:39,739 of the matrix, the cross -covariance. So if a 877 00:40:39,739 --> 00:40:42,099 robot is driving straight ahead, but it knows 878 00:40:42,099 --> 00:40:44,500 its steering is a bit wobbly, the ellipse stretches 879 00:40:44,500 --> 00:40:47,019 out laterally, perpendicular to the robot's motion. 880 00:40:47,940 --> 00:40:51,000 Precisely. As the robot drives blind, relying 881 00:40:51,000 --> 00:40:55,219 only on odometry, that covariance ellipse swells. 882 00:40:55,420 --> 00:40:57,920 It grows larger and larger, consuming the map, 883 00:40:58,300 --> 00:41:00,460 mathematically visualizing the fact that the 884 00:41:00,460 --> 00:41:03,760 robot is becoming completely lost. So how do 885 00:41:03,760 --> 00:41:06,139 we stop the bleeding? How do we collapse that 886 00:41:06,139 --> 00:41:08,219 giant ellipse of uncertainty back down to a tight 887 00:41:08,219 --> 00:41:10,739 little dot? We have to open the robot's eyes. 888 00:41:10,860 --> 00:41:12,860 We have to look outside the internal sensors 889 00:41:12,860 --> 00:41:16,420 and observe the external world. We localize against 890 00:41:16,420 --> 00:41:19,320 a landmark map. Let's say the robot has a LIDAR 891 00:41:19,320 --> 00:41:21,630 scanner. And it has a digital map that says there 892 00:41:21,630 --> 00:41:24,190 is a concrete pillar located at exactly coordinate 893 00:41:24,190 --> 00:41:27,909 20, 20. OK. The LIDAR pings the pillar. It measures 894 00:41:27,909 --> 00:41:30,969 the range, say, five meters away. And the bearing, 895 00:41:31,289 --> 00:41:33,869 say, 30 degrees to the right. And because the 896 00:41:33,869 --> 00:41:36,369 pillar's location on the map is considered absolute 897 00:41:36,369 --> 00:41:39,269 truth, the robot can just run the geometry backward. 898 00:41:39,630 --> 00:41:42,590 If that pillar is at 20, 20, and it is five meters 899 00:41:42,590 --> 00:41:45,860 away from me, then I must be exactly here. The 900 00:41:45,860 --> 00:41:48,139 observation allows the robot to instantly calculate 901 00:41:48,139 --> 00:41:50,820 its true global position, overriding the flawed 902 00:41:50,820 --> 00:41:53,679 odometry and collapsing the covariance ellipse. 903 00:41:54,139 --> 00:41:56,519 But doing this continuously, fusing the noisy 904 00:41:56,519 --> 00:41:59,179 drifting odometry with the noisy imperfect LiDAR 905 00:41:59,179 --> 00:42:01,940 measurements requires a very specific mathematical 906 00:42:01,940 --> 00:42:04,940 engine. It's the centerpiece of Chapter 6, the 907 00:42:04,940 --> 00:42:08,340 Extended Cullman Filter. The Kalman filter, originally 908 00:42:08,340 --> 00:42:11,019 developed for the Apollo space program, is arguably 909 00:42:11,019 --> 00:42:13,880 the most important algorithm in estimation theory. 910 00:42:14,300 --> 00:42:17,159 It is a recursive algorithm that finds the optimal 911 00:42:17,159 --> 00:42:19,719 estimate of a system's true state by balancing 912 00:42:19,719 --> 00:42:22,800 multiple sources of noisy data. Let's break open 913 00:42:22,800 --> 00:42:25,380 the black box. How does the Kalman filter actually 914 00:42:25,380 --> 00:42:28,280 negotiate this in practice? It operates in a 915 00:42:28,280 --> 00:42:31,460 continuous two -step rhythm. The first step is 916 00:42:31,460 --> 00:42:34,260 the predict step. The robot pushes its state 917 00:42:34,260 --> 00:42:36,679 forward in time using its physical models and 918 00:42:36,679 --> 00:42:39,300 odometry. I commanded the wheels to turn, so 919 00:42:39,300 --> 00:42:41,739 I predict I have moved forward one meter. My 920 00:42:41,739 --> 00:42:44,820 new estimated pose is X, and my covariance ellipse 921 00:42:44,820 --> 00:42:47,639 has grown. That's the dead reckoning part. We 922 00:42:47,639 --> 00:42:50,159 predict we moved, and we acknowledge we are more 923 00:42:50,159 --> 00:42:53,320 uncertain. What is step two? Step two is the 924 00:42:53,320 --> 00:42:56,400 update step. The robot takes a reading from its 925 00:42:56,400 --> 00:42:58,539 LIDAR. The Kalman filter does something very 926 00:42:58,539 --> 00:43:00,980 clever here. It doesn't just look at the LIDAR 927 00:43:00,980 --> 00:43:04,000 data blindly, It uses its predicted position 928 00:43:04,000 --> 00:43:06,139 to calculate where it thinks the landmark should 929 00:43:06,139 --> 00:43:09,579 be. If I am where I predict I am, the LiDAR should 930 00:43:09,579 --> 00:43:12,119 see the pillar at exactly 45 degrees. But the 931 00:43:12,119 --> 00:43:14,900 LiDAR actually sees the pillar at, say, 48 degrees. 932 00:43:15,159 --> 00:43:17,940 That three degree difference is the error. We 933 00:43:17,940 --> 00:43:19,900 call it the innovation. The innovation is the 934 00:43:19,900 --> 00:43:22,739 gap between the robot's internal belief and the 935 00:43:22,739 --> 00:43:24,960 external reality. Now, the magic of the Kalman 936 00:43:24,960 --> 00:43:28,199 filter happens in a calculation called the Kalman 937 00:43:28,199 --> 00:43:30,860 gain. What does the Kalman gain do? The filter 938 00:43:30,860 --> 00:43:34,099 has to decide. Do I trust my internal prediction 939 00:43:34,099 --> 00:43:37,539 or do I trust the external LiDAR? How does a 940 00:43:37,539 --> 00:43:39,519 math equation decide who to trust? It's just 941 00:43:39,519 --> 00:43:42,460 numbers. It looks at the variances. It compares 942 00:43:42,460 --> 00:43:45,619 the covariance matrix of the robot's prediction 943 00:43:45,619 --> 00:43:48,880 against the known noise profile of the LiDAR 944 00:43:48,880 --> 00:43:51,380 sensor. If the robot has been driving blind for 945 00:43:51,380 --> 00:43:54,239 an hour, its prediction variance is massive. 946 00:43:54,760 --> 00:43:56,840 The Kalman gain shifts heavily toward the sensor. 947 00:43:57,480 --> 00:44:00,719 It takes that three degree innovation and forcefully 948 00:44:00,719 --> 00:44:04,119 mathematically yanks the robot's estimated position 949 00:44:04,119 --> 00:44:07,659 back toward the LIDAR's reality, collapsing the 950 00:44:07,659 --> 00:44:09,900 ellipse. But what if it's the other way around? 951 00:44:10,340 --> 00:44:12,460 What if the robot just updated its position a 952 00:44:12,460 --> 00:44:15,159 millisecond ago? So its odometry's rock solid, 953 00:44:15,480 --> 00:44:17,360 but the LIDAR is looking through a cloud of smoke 954 00:44:17,360 --> 00:44:21,219 and returning highly noisy data. Then the sensor 955 00:44:21,219 --> 00:44:23,420 variance is higher than the prediction variance. 956 00:44:23,679 --> 00:44:26,380 The Kalman game drops to near zero. The filter 957 00:44:26,380 --> 00:44:29,239 essentially says, I trust my internal math more 958 00:44:29,239 --> 00:44:31,969 than this noisy laser reflection. It ignores 959 00:44:31,969 --> 00:44:34,230 the observation and keeps trusting the odometry. 960 00:44:34,929 --> 00:44:37,949 It is a brilliant, dynamic statistical balancing 961 00:44:37,949 --> 00:44:41,690 act. Predict, update. Predict, update. It continually 962 00:44:41,690 --> 00:44:44,090 refines the state estimate. It really is brilliant. 963 00:44:44,429 --> 00:44:47,809 But maybe I'm missing something here. I have 964 00:44:47,809 --> 00:44:50,789 a massive logic problem with this. What's that? 965 00:44:51,170 --> 00:44:53,170 The math assumes the LIDAR knows exactly what 966 00:44:53,170 --> 00:44:55,769 it's looking at. The LIDAR is just shooting photons 967 00:44:55,769 --> 00:44:58,150 and measuring the time of flight. It just sees 968 00:44:58,150 --> 00:45:00,550 a vertical cylinder. In a warehouse, there might 969 00:45:00,550 --> 00:45:03,769 be 50 identical concrete pillars. If the Kalman 970 00:45:03,769 --> 00:45:06,550 filter thinks it's looking at pillar A, but it's 971 00:45:06,550 --> 00:45:10,130 actually looking at pillar B. You have just identified 972 00:45:10,130 --> 00:45:12,949 the single most catastrophic failure mode in 973 00:45:12,949 --> 00:45:15,489 localization. It is called the data association 974 00:45:15,489 --> 00:45:18,369 problem. Yeah, because the LIDAR says, I see 975 00:45:18,369 --> 00:45:21,670 a pillar five meters away, but which one? The 976 00:45:21,670 --> 00:45:24,130 Kalman filter math relies entirely on matching 977 00:45:24,130 --> 00:45:26,170 the observation to the correct coordinate in 978 00:45:26,170 --> 00:45:28,969 the map. If the data association is wrong, the 979 00:45:28,969 --> 00:45:31,329 consequences are violent. If the robot thinks 980 00:45:31,329 --> 00:45:33,429 it is looking at pillar A, which should be two 981 00:45:33,429 --> 00:45:35,090 meters away, but is actually looking at pillar 982 00:45:35,090 --> 00:45:38,650 B, which is 15 meters away, the calculated innovation, 983 00:45:39,030 --> 00:45:40,789 that gap between belief and reality, is going 984 00:45:40,789 --> 00:45:43,389 to be massive. And because the LiDAR sensor is 985 00:45:43,389 --> 00:45:45,590 generally highly trusted, the Kalman gain is 986 00:45:45,590 --> 00:45:47,909 going to take that massive error and apply it 987 00:45:47,909 --> 00:45:50,429 to the robot's pose. The math will literally 988 00:45:50,429 --> 00:45:53,250 teleport the robot's estimated position 13 meters 989 00:45:53,250 --> 00:45:55,570 across the room, shoving it through virtual walls 990 00:45:55,570 --> 00:45:57,730 and completely destroying its localization state. 991 00:45:58,329 --> 00:46:00,309 The robot will try to execute path planning from 992 00:46:00,309 --> 00:46:02,730 a location it isn't actually in, resulting in 993 00:46:02,730 --> 00:46:05,489 a physical crash. So how do we prevent the math 994 00:46:05,489 --> 00:46:07,809 from destroying the robot? How do we verify the 995 00:46:07,809 --> 00:46:10,630 identity of the pillar? We use statistical gating. 996 00:46:11,250 --> 00:46:13,369 Before the Kalman filter is allowed to use an 997 00:46:13,369 --> 00:46:15,829 observation, it has to pass a strict confidence 998 00:46:15,829 --> 00:46:20,550 test called the Mahalanobis distance. Mahalanobis 999 00:46:20,550 --> 00:46:22,650 distance. How is that different from regular 1000 00:46:22,650 --> 00:46:25,809 Euclidean distance? Euclidean distance is just 1001 00:46:25,809 --> 00:46:27,989 a ruler. It measures straight line distance. 1002 00:46:28,590 --> 00:46:30,909 Mahalanobis distance measures how many standard 1003 00:46:30,909 --> 00:46:33,289 deviations away a point is from a distribution. 1004 00:46:33,889 --> 00:46:36,510 It accounts for the shape of the covariance ellipse. 1005 00:46:36,730 --> 00:46:39,510 When the LiDAR sees a blip, the robot draws a 1006 00:46:39,510 --> 00:46:41,769 statistical boundary around its predicted location 1007 00:46:41,769 --> 00:46:44,190 for pillar A based on its current uncertainty. 1008 00:46:44,590 --> 00:46:48,360 Like a confidence interval. Exactly. If the LIDAR 1009 00:46:48,360 --> 00:46:50,639 blip falls outside that boundary, meaning the 1010 00:46:50,639 --> 00:46:53,179 Mahalanobis distance is too large, the robot 1011 00:46:53,179 --> 00:46:55,340 concludes that the blip is highly unlikely to 1012 00:46:55,340 --> 00:46:58,059 be Pillar A. It's an anomaly. It could be Pillar 1013 00:46:58,059 --> 00:47:00,579 B, it could be a person walking by, whatever. 1014 00:47:00,840 --> 00:47:03,260 Right. And the golden rule of localization is, 1015 00:47:03,639 --> 00:47:05,599 if you cannot confidently associate the data, 1016 00:47:05,960 --> 00:47:08,920 you discard it. It is vastly safer to ignore 1017 00:47:08,920 --> 00:47:11,719 the LiDAR and rely on your drifting odometry 1018 00:47:11,719 --> 00:47:14,500 for a few more seconds than to feed a mismatched 1019 00:47:14,500 --> 00:47:17,420 landmark into the Kalman filter and risk a catastrophic 1020 00:47:17,420 --> 00:47:19,880 pose jump. That makes perfect sense. Better safe 1021 00:47:19,880 --> 00:47:22,519 than teleporting into a wall. Okay, so we have 1022 00:47:22,519 --> 00:47:25,480 the Kalman filter. We are managing our uncertainty. 1023 00:47:26,199 --> 00:47:30,400 We are gating our data association. We can localize. 1024 00:47:30,639 --> 00:47:33,599 But there is a glaring assumption we've been 1025 00:47:33,599 --> 00:47:36,690 making this entire time. We assumed we had a 1026 00:47:36,690 --> 00:47:39,710 map. Right. We assumed someone handed the robot 1027 00:47:39,710 --> 00:47:42,329 a pristine, accurate digital blueprint of every 1028 00:47:42,329 --> 00:47:44,929 pillar and tree in the environment. What if the 1029 00:47:44,929 --> 00:47:48,110 robot wakes up on the surface of Mars? Or inside 1030 00:47:48,110 --> 00:47:50,809 a collapsed coal mine? There is no map. Which 1031 00:47:50,809 --> 00:47:53,530 brings us to the final, most complex topic in 1032 00:47:53,530 --> 00:47:58,590 mobile robotics. Segment four, SLAM. Simultaneous 1033 00:47:58,590 --> 00:48:01,530 localization and mapping. The ultimate chicken 1034 00:48:01,530 --> 00:48:03,949 and egg problem. To understand why SLAM is so 1035 00:48:03,949 --> 00:48:06,539 incredibly difficult, Cork first defines why 1036 00:48:06,539 --> 00:48:09,760 pure mapping is easy. If you have a perfect military 1037 00:48:09,760 --> 00:48:12,179 -grade GPS, your position is known with absolute 1038 00:48:12,179 --> 00:48:14,440 certainty. You just drive around, your lidar 1039 00:48:14,440 --> 00:48:16,559 pings a rock, you calculate the geometry, and 1040 00:48:16,559 --> 00:48:18,380 you write the rock's coordinates into a database. 1041 00:48:18,900 --> 00:48:21,699 You are mapping with known poses. Mathematically, 1042 00:48:21,940 --> 00:48:24,300 Cork notes that the covariance matrix for a pure 1043 00:48:24,300 --> 00:48:27,300 mapping problem is block diagonal. A block diagonal 1044 00:48:27,300 --> 00:48:29,860 matrix. Meaning the math for estimating rock 1045 00:48:29,860 --> 00:48:32,340 A is completely isolated from the math for estimating 1046 00:48:32,340 --> 00:48:36,159 rock B. The variables don't mix. Exactly. Because 1047 00:48:36,159 --> 00:48:39,019 your own position is a grounded truth, observing 1048 00:48:39,019 --> 00:48:41,119 rock A gives you absolutely no new information 1049 00:48:41,119 --> 00:48:43,920 about rock B. But SLAM removes that grounding 1050 00:48:43,920 --> 00:48:47,360 truth. You do not have GPS. You are relying on 1051 00:48:47,360 --> 00:48:49,699 messy odometry and you are trying to build a 1052 00:48:49,699 --> 00:48:52,659 map of unknown landmarks using sensors mounted 1053 00:48:52,659 --> 00:48:54,599 on a chassis that doesn't actually know where 1054 00:48:54,599 --> 00:48:57,800 it is. Cork uses a brilliant historical analogy 1055 00:48:57,800 --> 00:49:01,219 for this in the book. Captain James Cooke. In 1056 00:49:01,219 --> 00:49:04,860 1770, Cook mapped the Eora territory, which is 1057 00:49:04,860 --> 00:49:06,860 the complex coastline of modern -day Sydney, 1058 00:49:06,860 --> 00:49:10,039 Australia. Cook was essentially performing slam. 1059 00:49:10,829 --> 00:49:13,510 He was on a wooden ship. His odometry was terrible. 1060 00:49:13,710 --> 00:49:15,929 He was trying to estimate a speed through dynamic 1061 00:49:15,929 --> 00:49:18,190 ocean currents using a piece of wood tied to 1062 00:49:18,190 --> 00:49:20,989 a rope. Not exactly a high -precision wheel encoder. 1063 00:49:21,170 --> 00:49:23,650 No, not at all. His compass headings were noisy. 1064 00:49:23,969 --> 00:49:26,909 He only had very infrequent, imprecise celestial 1065 00:49:26,909 --> 00:49:30,300 fixes to anchor his global position. Yet... As 1066 00:49:30,300 --> 00:49:32,579 he sailed, he had to simultaneously calculate 1067 00:49:32,579 --> 00:49:34,880 his own trajectory while drawing an accurate 1068 00:49:34,880 --> 00:49:37,639 map of a totally unknown, jagged coastline based 1069 00:49:37,639 --> 00:49:40,079 purely on what he could see from the deck. If 1070 00:49:40,079 --> 00:49:42,420 he thinks he sailed a mile north, but a current 1071 00:49:42,420 --> 00:49:45,139 actually pushed him a mile east, every single 1072 00:49:45,139 --> 00:49:46,880 mountain he draws on his map is going to be plotted 1073 00:49:46,880 --> 00:49:49,860 a mile in the wrong direction. The map is completely 1074 00:49:49,860 --> 00:49:53,090 corrupted by his positional uncertainty. So how 1075 00:49:53,090 --> 00:49:55,289 does the Kalman filter solve this? It solves 1076 00:49:55,289 --> 00:49:58,570 it by expanding the state vector. In basic localization, 1077 00:49:58,789 --> 00:50:00,969 the Kalman filter is only estimating three numbers, 1078 00:50:01,230 --> 00:50:05,110 the robots X, Y, and heading. In EKF SLAM, the 1079 00:50:05,110 --> 00:50:07,929 filter estimates the robot's pose plus the X 1080 00:50:07,929 --> 00:50:10,670 and Y coordinates of every single landmark it 1081 00:50:10,670 --> 00:50:13,510 ever sees. The state vector grows continuously 1082 00:50:13,510 --> 00:50:15,849 as the robot explores. It's trying to balance 1083 00:50:15,849 --> 00:50:18,949 a massive equation where every single variable 1084 00:50:18,949 --> 00:50:21,869 is uncertain. And this is where the covariance 1085 00:50:21,869 --> 00:50:25,090 matrix becomes magical. In SLAM, the covariance 1086 00:50:25,090 --> 00:50:27,750 matrix is no longer block diagonal. There are 1087 00:50:27,750 --> 00:50:31,190 no zeros. The matrix is fully correlated. Every 1088 00:50:31,190 --> 00:50:34,230 single off -diagonal element has a value. Meaning 1089 00:50:34,230 --> 00:50:36,829 every single object in the map is mathematically 1090 00:50:36,829 --> 00:50:40,050 tethered to every other object. Think of it like 1091 00:50:40,050 --> 00:50:42,860 a web of invisible springs. Because the robot's 1092 00:50:42,860 --> 00:50:45,139 position is uncertain, when it observes landmark 1093 00:50:45,139 --> 00:50:48,539 A, landmark A's estimated position inherits the 1094 00:50:48,539 --> 00:50:51,099 robot's uncertainty. Later, it observes landmark 1095 00:50:51,099 --> 00:50:54,239 B. Landmark B also inherits that same uncertainty. 1096 00:50:54,440 --> 00:50:57,099 The errors are shared. The landmarks become deeply 1097 00:50:57,099 --> 00:50:58,820 correlated with each other through the history 1098 00:50:58,820 --> 00:51:01,119 of the robot's movement. So let's play this out. 1099 00:51:01,559 --> 00:51:03,420 Let's say the robot drives in a large circle 1100 00:51:03,420 --> 00:51:06,260 for 20 minutes. Its dead reckoning is drifting 1101 00:51:06,260 --> 00:51:08,860 the entire time, so the map it's building is 1102 00:51:08,860 --> 00:51:12,000 slowly warping. But then... It turns a corner, 1103 00:51:12,420 --> 00:51:16,079 and its LIDAR sees landmark A again, the very 1104 00:51:16,079 --> 00:51:18,539 first landmark it mapped. This is called a loop 1105 00:51:18,539 --> 00:51:21,679 closure, and it is the most important event in 1106 00:51:21,679 --> 00:51:24,559 SLAM. The robot's dead reckoning says landmark 1107 00:51:24,559 --> 00:51:26,960 A should be five meters to the left, but the 1108 00:51:26,960 --> 00:51:29,400 LIDAR sees landmark A five meters to the right. 1109 00:51:29,840 --> 00:51:32,420 The robot realizes its odometry has been lying 1110 00:51:32,420 --> 00:51:35,500 to it for 20 minutes. The Kalman filter calculates 1111 00:51:35,500 --> 00:51:38,380 a massive innovation. It updates the robot's 1112 00:51:38,380 --> 00:51:40,579 estimated position, violently pulling it back 1113 00:51:40,579 --> 00:51:43,699 to reality. But here is the beauty of the fully 1114 00:51:43,699 --> 00:51:46,429 correlated covariance matrix. When the Kalman 1115 00:51:46,429 --> 00:51:49,469 filter pulls the robot's pose, the invisible 1116 00:51:49,469 --> 00:51:51,849 springs in the matrix instantly transmit that 1117 00:51:51,849 --> 00:51:54,789 tension across the entire map. Because all the 1118 00:51:54,789 --> 00:51:56,590 landmarks were mapped from that drifting pose, 1119 00:51:56,909 --> 00:51:59,110 when the pose is corrected, the mathematical 1120 00:51:59,110 --> 00:52:01,690 springs pull the estimated positions of landmark 1121 00:52:01,690 --> 00:52:04,230 B, landmark C, and every other landmark the robot 1122 00:52:04,230 --> 00:52:06,730 has seen, instantly shifting them into a tighter, 1123 00:52:06,769 --> 00:52:09,809 more accurate configuration. The entire warped 1124 00:52:09,809 --> 00:52:14,030 map snaps into focus. It's a symphony of statistical 1125 00:52:14,030 --> 00:52:17,940 correlation. Something. That is so cool. But 1126 00:52:17,940 --> 00:52:20,519 Cork points out that EKF Slam has a hard ceiling. 1127 00:52:20,920 --> 00:52:24,239 It cannot scale. The computational cost is its 1128 00:52:24,239 --> 00:52:26,800 downfall. Because the matrix is fully correlated 1129 00:52:26,800 --> 00:52:29,260 every time the robot observes a single landmark, 1130 00:52:29,579 --> 00:52:32,119 the Kalman filter must update every single element 1131 00:52:32,119 --> 00:52:35,539 in that massive matrix. If you map 10,000 landmarks 1132 00:52:35,539 --> 00:52:38,380 in a city, you have a 10,000 by 10,000 matrix. 1133 00:52:38,639 --> 00:52:42,320 That's 100 million elements. Updating that dense 1134 00:52:42,320 --> 00:52:45,059 grid of covariance math 30 times a second will 1135 00:52:45,059 --> 00:52:48,139 melt a standard processor. Furthermore, the extended 1136 00:52:48,139 --> 00:52:51,039 Kalman filter requires linearization. It uses 1137 00:52:51,039 --> 00:52:54,260 Taylor series expansions to force nonlinear geometry 1138 00:52:54,260 --> 00:52:58,000 into linear matrix math. Over time, those approximation 1139 00:52:58,000 --> 00:53:01,599 errors build up and destroy the map. So the math 1140 00:53:01,599 --> 00:53:03,800 that got us to the moon isn't enough to map a 1141 00:53:03,800 --> 00:53:08,900 city. Modern robotics had to evolve. Cork introduces 1142 00:53:08,900 --> 00:53:11,219 two modern alternatives that bypass the limits 1143 00:53:11,219 --> 00:53:15,519 of EKF SLAM. The first is PoseGraphSLAM. How 1144 00:53:15,519 --> 00:53:19,179 does this solve the matrix overload? PoseGraphSLAM 1145 00:53:19,179 --> 00:53:21,360 abandons the idea of updating a dense, fully 1146 00:53:21,360 --> 00:53:23,840 correlated matrix in real time. Instead, it turns 1147 00:53:23,840 --> 00:53:26,400 the robot's trajectory into a mathematical graph, 1148 00:53:26,539 --> 00:53:28,360 just like the navigation planners we discussed 1149 00:53:28,360 --> 00:53:31,239 earlier. The nodes in this graph are the robot's 1150 00:53:31,239 --> 00:53:33,860 historical poses, where it was at time 0, time 1151 00:53:33,860 --> 00:53:37,480 1, time 2. The edges connecting those nodes are 1152 00:53:37,480 --> 00:53:40,079 constraints generated by the sensors. Like, my 1153 00:53:40,079 --> 00:53:42,920 odometry says pose 2 is exactly 1 meter in front 1154 00:53:42,920 --> 00:53:47,619 of pose 1. That's an edge. Exactly. Or, my LIDAR 1155 00:53:47,619 --> 00:53:50,559 saw the same tree from pose 10 and pose 50, so 1156 00:53:50,559 --> 00:53:53,139 those two poses must be exactly 5 meters apart. 1157 00:53:53,949 --> 00:53:56,349 The system is divided into two halves. The front 1158 00:53:56,349 --> 00:53:59,010 end just casually extracts features and builds 1159 00:53:59,010 --> 00:54:01,389 this graph of nodes and edges as the robot drives. 1160 00:54:01,579 --> 00:54:03,639 It doesn't stress about making the map perfect 1161 00:54:03,639 --> 00:54:05,980 in real time. It just builds the web of constraints. 1162 00:54:06,500 --> 00:54:09,530 It's just collecting the raw, messy data. Then 1163 00:54:09,530 --> 00:54:12,389 the back end runs a heavy optimization algorithm, 1164 00:54:12,730 --> 00:54:15,590 usually nonlinear least squares. It looks at 1165 00:54:15,590 --> 00:54:18,949 the entire tangled, contradictory graph of springs, 1166 00:54:19,269 --> 00:54:21,630 and it iteratively shifts all the historical 1167 00:54:21,630 --> 00:54:24,010 poses around until the tension across all the 1168 00:54:24,010 --> 00:54:26,199 springs is minimized. Because the connections 1169 00:54:26,199 --> 00:54:28,760 are sparse, pose 50 is only connected to pose 1170 00:54:28,760 --> 00:54:32,159 49 and pose 10, not to pose 2, the math uses 1171 00:54:32,159 --> 00:54:34,559 sparse matrices, which are incredibly fast to 1172 00:54:34,559 --> 00:54:37,219 solve. It scales beautifully to massive environments. 1173 00:54:37,980 --> 00:54:40,119 It optimizes the entire history of the robot 1174 00:54:40,119 --> 00:54:43,920 at once. That's so much more efficient. The second 1175 00:54:43,920 --> 00:54:46,239 modern solution Cork brings up is sequential 1176 00:54:46,239 --> 00:54:49,340 Monte Carlo, commonly known as particle filters. 1177 00:54:50,460 --> 00:54:53,820 If pose graph slam is elegant optimization, particle 1178 00:54:53,820 --> 00:54:56,530 filters sound like brute force computational 1179 00:54:56,530 --> 00:54:59,269 violence. They are a brilliant application of 1180 00:54:59,269 --> 00:55:01,929 raw computing power. A Kalman filter tries to 1181 00:55:01,929 --> 00:55:04,190 elegantly track a single Gaussian ellipse of 1182 00:55:04,190 --> 00:55:06,750 uncertainty. A particle filter abandons the ellipse 1183 00:55:06,750 --> 00:55:09,449 entirely. When the robot wakes up and doesn't 1184 00:55:09,449 --> 00:55:12,090 know where it is, the algorithm scatters thousands, 1185 00:55:12,349 --> 00:55:15,030 sometimes tens of thousands, of digital particles 1186 00:55:15,030 --> 00:55:17,550 randomly across the digital map. Every single 1187 00:55:17,550 --> 00:55:20,289 particle represents a hypothesis. Like, maybe 1188 00:55:20,289 --> 00:55:22,429 I am this particle in the hallway. Maybe I am 1189 00:55:22,429 --> 00:55:24,400 this particle in the corner. You basically have 1190 00:55:24,400 --> 00:55:27,119 a swarm of thousands of virtual ghost robots 1191 00:55:27,119 --> 00:55:29,760 inside the computer's memory. As the real physical 1192 00:55:29,760 --> 00:55:32,039 robot drives forward one meter, the algorithm 1193 00:55:32,039 --> 00:55:34,579 commands every single one of those thousands 1194 00:55:34,579 --> 00:55:37,460 of ghost particles to also drive forward one 1195 00:55:37,460 --> 00:55:40,360 meter on the map. But the real robot is driving 1196 00:55:40,360 --> 00:55:43,960 with noisy wheels, so it injects simulated noise 1197 00:55:43,960 --> 00:55:46,539 into the ghosts, right? Some ghosts drive 1 .1 1198 00:55:46,539 --> 00:55:48,800 meters, some drive 0 .9 meters, some veer slightly 1199 00:55:48,800 --> 00:55:52,400 left. The swarm naturally spreads out. Then comes 1200 00:55:52,400 --> 00:55:54,849 the update step. The real robot takes a LiDAR 1201 00:55:54,849 --> 00:55:57,010 reading and sees a wall exactly 2 meters in front 1202 00:55:57,010 --> 00:55:59,670 of it. The algorithm then iterates through the 1203 00:55:59,670 --> 00:56:02,110 swarm. It looks at particle number 1 and asks 1204 00:56:02,110 --> 00:56:04,909 the map, if the robot is standing exactly where 1205 00:56:04,909 --> 00:56:07,230 particle number 1 is, how far away should the 1206 00:56:07,230 --> 00:56:09,670 wall be? If the map says the wall is 10 meters 1207 00:56:09,670 --> 00:56:12,489 away from particle number 1, but the real LiDAR 1208 00:56:12,489 --> 00:56:14,929 is seeing the wall at 2 meters, particle number 1209 00:56:14,929 --> 00:56:17,349 1 is a terrible hypothesis. So the algorithm 1210 00:56:17,349 --> 00:56:20,210 assigns it a very low weight. It checks particle 1211 00:56:20,210 --> 00:56:23,230 number 2. The map says the wall should be 2 .1 1212 00:56:23,230 --> 00:56:25,849 meters away. That's a very good hypothesis. It 1213 00:56:25,849 --> 00:56:28,969 gets a high weight. Once all particles are evaluated, 1214 00:56:29,150 --> 00:56:31,750 the algorithm performs a resampling step. This 1215 00:56:31,750 --> 00:56:34,250 is essentially Darwinian evolution applied to 1216 00:56:34,250 --> 00:56:37,619 statistics. Survival of the fittest data. The 1217 00:56:37,619 --> 00:56:39,519 algorithm deletes the low -weight particles, 1218 00:56:39,860 --> 00:56:41,920 the ghosts that were standing in the wrong spots, 1219 00:56:41,960 --> 00:56:44,579 and it duplicates or clones the high -weight 1220 00:56:44,579 --> 00:56:47,420 particles. As the robot moves and senses over 1221 00:56:47,420 --> 00:56:49,539 a few seconds, the particles in the wrong rooms 1222 00:56:49,539 --> 00:56:52,119 rapidly die off and the particles in the correct 1223 00:56:52,119 --> 00:56:55,199 room clone and multiply until the entire swarm 1224 00:56:55,199 --> 00:56:59,119 condenses into a tight, dense cluster precisely 1225 00:56:59,119 --> 00:57:01,840 over the robot's true location. And this can 1226 00:57:01,840 --> 00:57:04,280 actually solve the kidnapped robot problem, right? 1227 00:57:04,699 --> 00:57:06,800 If I physically pick the robot up and carry it 1228 00:57:06,800 --> 00:57:09,980 to another room, a Kalman filter will crash because 1229 00:57:09,980 --> 00:57:12,099 its single ellipse will be thrown into a wall. 1230 00:57:12,489 --> 00:57:14,909 But a particle filter will just realize all its 1231 00:57:14,909 --> 00:57:17,489 particles have terrible weights. It will scatter 1232 00:57:17,489 --> 00:57:20,010 new particles across the map and the swarm will 1233 00:57:20,010 --> 00:57:23,090 rapidly reconverge on the new location. It is 1234 00:57:23,090 --> 00:57:26,489 remarkably robust. It handles highly nonlinear 1235 00:57:26,489 --> 00:57:28,809 sensor noise perfectly because it isn't trying 1236 00:57:28,809 --> 00:57:31,469 to calculate a Gaussian bell curve. It's just 1237 00:57:31,469 --> 00:57:33,949 empirically testing reality thousands of times 1238 00:57:33,949 --> 00:57:36,989 a second. Which brings us to the main takeaway 1239 00:57:36,989 --> 00:57:39,429 from all of this. If I've learned anything from 1240 00:57:39,429 --> 00:57:41,710 tracing the path from chapter 4 to chapter 6. 1241 00:57:41,769 --> 00:57:44,869 from the bicycle model to the particle swarm. 1242 00:57:45,389 --> 00:57:48,690 It is that mobile robotics is an endless grueling 1243 00:57:48,690 --> 00:57:51,849 negotiation. Negotiation is the exact right word. 1244 00:57:52,449 --> 00:57:54,309 You cannot simply command a physical machine 1245 00:57:54,309 --> 00:57:57,230 to execute a mathematical ideal. You have to 1246 00:57:57,230 --> 00:57:59,250 negotiate with the mechanical constraints of 1247 00:57:59,250 --> 00:58:01,349 Ackerman steering. You have to negotiate with 1248 00:58:01,349 --> 00:58:03,610 the computational limits of grid planners by 1249 00:58:03,610 --> 00:58:06,590 using D star or random sampling. You have to 1250 00:58:06,590 --> 00:58:09,809 negotiate with the physics of inertia using clothoid 1251 00:58:09,809 --> 00:58:12,000 spiral so you don't flip the robot. And finally, 1252 00:58:12,159 --> 00:58:14,400 you have to constantly negotiate with the messy 1253 00:58:14,400 --> 00:58:17,159 shifting reality of the world using Kalman filters 1254 00:58:17,159 --> 00:58:20,619 and SLAM algorithms to tame the existential uncertainty 1255 00:58:20,619 --> 00:58:23,840 of localization. SLAM is the ultimate mathematical 1256 00:58:23,840 --> 00:58:25,960 tether that keeps the digital brain anchored 1257 00:58:25,960 --> 00:58:29,099 to the physical world. We have successfully navigated 1258 00:58:29,099 --> 00:58:31,530 the environment. We've mastered moving the entire 1259 00:58:31,530 --> 00:58:33,969 chassis through space. Next time in Part 3 of 1260 00:58:33,969 --> 00:58:36,469 5, Robot Manipulators, we are going to anchor 1261 00:58:36,469 --> 00:58:38,469 that mobile base to the floor and we are going 1262 00:58:38,469 --> 00:58:40,889 to dive into the kinematics of moving the robotic 1263 00:58:40,889 --> 00:58:43,630 arm itself. Moving from the macro -navigation 1264 00:58:43,630 --> 00:58:46,670 of a warehouse to the micro -precision of a manipulator 1265 00:58:46,670 --> 00:58:49,269 brings a whole new set of geometric challenges. 1266 00:58:49,690 --> 00:58:51,949 But before we sign off, I want to leave you with 1267 00:58:51,949 --> 00:58:55,190 a final thought to ponder. Throughout this entire 1268 00:58:55,190 --> 00:58:57,929 deep dive on SLAM and grid planners, we made 1269 00:58:57,929 --> 00:59:01,309 a massive unspoken assumption. We assumed the 1270 00:59:01,309 --> 00:59:04,309 obstacles in the environment were static. The 1271 00:59:04,309 --> 00:59:06,889 walls, the concrete pillars, the trees, they 1272 00:59:06,889 --> 00:59:10,460 don't move. But what happens to a D -star wavefront 1273 00:59:10,460 --> 00:59:14,980 or a PRM graph or a SLAM covariance matrix when 1274 00:59:14,980 --> 00:59:18,369 the obstacles in your map are humans walking 1275 00:59:18,369 --> 00:59:20,849 through a warehouse, or a fleet of other robots 1276 00:59:20,849 --> 00:59:24,130 all simultaneously calculating and recalculating 1277 00:59:24,130 --> 00:59:26,489 their own paths through the same exact space. 1278 00:59:27,269 --> 00:59:30,090 How do you mathematically map and navigate a 1279 00:59:30,090 --> 00:59:33,750 world that refuses to stand still? Keep unpacking 1280 00:59:33,750 --> 00:59:35,510 that. Thanks for joining us on the Deep Dive.