Robotics Fundamentals 1, Chapter 8: Challenge 3 solution (occupancy grids, planning, localisation and stopping safely) Formulas: log-odds step = ln( p / ( 1 - p ) ) ; l = l + step ; p = 1 / ( 1 + e^(-l) ) A*: f = g + h , h = Manhattan distance |dr| + |dc| ; pick the lowest f, then the lowest h, then the cell added first trilateration (A at the origin, B = (b, 0), C = (0, c)): x = ( dA^2 - dB^2 + b^2 ) / ( 2 b ) , y = ( dA^2 - dC^2 + c^2 ) / ( 2 c ) particle weight = exp( -( z - predicted )^2 / ( 2 sigma^2 ) ) stopping distance = v x latency + v^2 / ( 2 a ) (a) 80 % sensor model: step = ln( 0.8 / 0.2 ) = ln 4 = +1.386 for a hit, -1.386 for a pass-through. Readings hit, hit, free, hit, hit: reading hit hit free hit hit l 1.386 2.773 1.386 2.773 4.159 p 0.800 0.941 0.800 0.941 0.985 (Net three hits: 3 x 1.386 = 4.159, p = 1 / ( 1 + e^-4.159 ) = 0.9846. With a clamp of +-4 the last value would be held at +4, p = 0.982.) Four pass-throughs from the prior 0.5: l = -4 x 1.386 = -5.545, p = 1 / ( 1 + e^5.545 ) = 0.0039 (clamped to -4: p = 0.018). Clamp limits: p( +4 ) = 1 / ( 1 + e^-4 ) = 0.982 and p( -4 ) = 1 / ( 1 + e^4 ) = 0.018. A cell can never become more certain than that, so a few contrary readings can still change it. (b) Grid (row, column), cells 0.2 m, moves in the order up, right, down, left: c0 c1 c2 c3 c4 r0 S . # . . r1 . . # . G r2 . . . # . r3 # . . . . S = (0,0), G = (1,4). Manhattan distance S to G = |1 - 0| + |4 - 0| = 5 (the walls make the real route longer). BFS: the number of moves from S, written in each cell it reached before G came off the queue (. = never reached): c0 c1 c2 c3 c4 r0 0 1 # . . r1 1 2 # . 9 r2 2 3 4 # 8 r3 # 4 5 6 7 9 moves to G. 13 cells were taken off the queue (all the cells shown), the goal last. A*: h(r,c) = |r - 1| + |c - 4|, h(S) = 5. # expand g h f added or improved (cell: g, h, f) 1 (0,0) 0 5 5 (0,1): 1,4,5 (1,0): 1,4,5 2 (0,1) 1 4 5 (1,1): 2,3,5 (to the right is a wall; tie with (1,0) at f = 5: (0,1) has the same h, 4, and was added first) 3 (1,1) 2 3 5 (2,1): 3,4,7 (the wall to the right; (1,0) already has g = 1) 4 (1,0) 1 4 5 (2,0): 2,5,7 (the only cell left with f = 5) 5 (2,1) 3 4 7 (2,2): 4,3,7 (3,1): 4,5,9 (f has risen from 5 to 7: the detour begins; ties at f = 7 go to the lowest h, 4) 6 (2,2) 4 3 7 (3,2): 5,4,9 (the wall is to the right) 7 (2,0) 2 5 7 none (the last f = 7) 8 (3,2) 5 4 9 (3,3): 6,3,9 ((3,1) also has f = 9 but h = 5, so (3,2) goes first) 9 (3,3) 6 3 9 (3,4): 7,2,9 10 (3,4) 7 2 9 (2,4): 8,1,9 11 (2,4) 8 1 9 (1,4): 9,0,9 12 (1,4) 9 0 9 GOAL reached, path cost g = 9 Path (back from G by the recorded parents): (0,0) (0,1) (1,1) (2,1) (2,2) (3,2) (3,3) (3,4) (2,4) (1,4) 9 moves x 0.2 m = 1.8 m. A* expanded 12 cells against 13 for BFS: almost no saving on this grid, because the walls force every route through the same narrow corridor and there is nowhere for BFS to waste effort. The Manhattan guess of 5 rose to the true cost 9 (f went 5, then 7, then 9) as the search met each wall: the heuristic is always at or below the truth, which is why the answer is a shortest path. (A* saves most on open floor, such as the 6 by 5 grid in the chapter, where it expanded 14 cells against 25.) (c) A = (0, 0), B = (5, 0), C = (0, 4); dA = 3.20, dB = 3.90, dC = 2.50. x = ( 3.20^2 - 3.90^2 + 5^2 ) / ( 2 x 5 ) = ( 10.24 - 15.21 + 25 ) / 10 = 20.03 / 10 = 2.003 m y = ( 3.20^2 - 2.50^2 + 4^2 ) / ( 2 x 4 ) = ( 10.24 - 6.25 + 16 ) / 8 = 19.99 / 8 = 2.499 m Position (2.003, 2.499). The true position is (2.0, 2.5): the error is about 3 mm. (True ranges: 3.202, 3.905 and 2.500 m, rounded to 3.20, 3.90 and 2.50 in the readings.) (d) Wall at x = 6.0 m, z = 3.6 m, sigma = 0.5 m. Predicted range = 6.0 - x; weight = exp( -( z - predicted )^2 / ( 2 x 0.25 ) ) = exp( -( z - predicted )^2 / 0.5 ). particle x predicted z - predicted weight normalised expected copies of 5 1 5.0 -1.4 0.0198 0.016 0.08 2 4.0 -0.4 0.7261 0.586 2.93 3 3.0 +0.6 0.4868 0.393 1.96 4 2.0 +1.6 0.0060 0.005 0.02 5 1.0 +2.6 0.0000 0.000 0.00 (exp( -13.52 ) = 0.0000013) Sum of weights = 1.2387. Normalised weight = weight / 1.2387. Expected copies = 5 x normalised. Weighted mean = 1 x 0.016 + 2 x 0.586 + 3 x 0.393 + 4 x 0.005 + 5 x 0.000 = 2.387 m. (The reading z = 3.6 m means the robot is at x = 6.0 - 3.6 = 2.4 m: the estimate is close. With only five particles the result lies between the two heaviest ones, at x = 2 and 3. More particles would give a finer answer.) (e) v = 0.8 m/s, a = 2.5 m/s^2, total reaction time 60 ms. Reaction distance = 0.8 x 0.06 = 0.048 m. Braking distance = 0.8^2 / ( 2 x 2.5 ) = 0.64 / 5 = 0.128 m. Stopping distance = 0.048 + 0.128 = 0.176 m. Threshold with a margin of 2: 2 x 0.176 = 0.352 m. Largest speed that leaves a margin of 2 with a 0.30 m threshold: the stopping distance must not exceed 0.15 m: v x 0.06 + v^2 / 5 = 0.15 -> v^2 + 0.3 v - 0.75 = 0 -> v = ( -0.3 + sqrt( 0.09 + 3.0 ) ) / 2 = ( -0.3 + 1.758 ) / 2 = 0.729 m/s. Check: 0.729 x 0.06 + 0.729^2 / 5 = 0.0437 + 0.1063 = 0.150 m. So with this threshold the speed should be limited to about 0.73 m/s, not 0.8 m/s.