The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance. Samuel Prentice and Nicholas Roy Presentation by Elaine Short

Transcription

1 The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance Samuel Prentice and Nicholas Roy Presentation by Elaine Short 1

2 Outline" Motivation Review of PRM and EKF Factoring the Covariance The Algorithm Experimental Validation 2

3 Motivation In the real world, a robotic agent will have uncertainty in its knowledge of its own state. To incorporate this into planning we can use POMDPs, but they are difficult to solve. Instead, use Probabilistic Road Maps, incorporating uncertainty using Extended Kalman Filtering. Now plan in belief space, minimizing cost which includes the uncertainty in the movement. 3

4 PRM Review Probabilistic Road Maps: Choose a bunch of points in the collision-free subset of the search space beacon C free sample Connect those points where there exists a straight line path between them in C!ee. (a) Sampled Distribution Means 4 Find shortest path on the resulting graph beacon C free sample START beacon PRM beacon edge! PRM ellipse node PRM edge START (a) Sampled Distribution Means (c) Resulting Belief Space Graph (d) (b) Edges Added Fig. 1. A basic example of building a belief-space roadmap in an environment with ranging beacons. (a C free are kept. (b) Edges between distributions that lie in C free are added to the graph. (c) Once the START) can be propagated through the graph by simulating the agent s motion and sensor measurements, 4 sequence along edges. The posterior distribution at each node is drawn with 1 σ uncertainty ellipses, and search path to that node. In GOAL this example, we artificially increased the noise in the robot motion to make the environment. Figure (d) reiterates the benefit of incorporating the full belief distribution in planning.

5 KF Review state = g(state, control, noise) measured state = h(state, noise) Robot state represented by a Gaussian (mean and covariance). Cov. of s (state) Control and observations are applied sequentially, are linear transforms on the Gaussian (generally, control will make it wider, and observation will make it narrower). Extended Kalman Filter linearizes the nonlinear control and observation functions. Ω t = Σ 1 t =(G t Σ t 1 G T t + R t ) 1 Ω t = Ω t + H T t Q 1 t H t. M t µ t = g(µ t 1,u t ) Σ t = G t Σ t 1 G T t Ht T Q 1 t H t + V t W t V T t µ t = µ t + K t (h(µ t ) z t ) Σ t = Σ t K t H t Σ t, Jacobian of g w.r.t s (state) Jacobian of g w.r.t w (noise) Cov. of w (noise) Jacobian of w R t V t W t V T t. 5

6 Kalman Filter 6

7 over its n-dimensional mean, then the reachable part of the collision (Figure 1b). We then simulate a sequence of controls The Intuition beacon PRM node PRM edge (b) Edges Added GOAL START beacon PRM edge! ellipse START beacon shortest path b space path! ellipse (c) Resulting Belief Space Graph (d) Advantage of Belief Space Planning Fig. 1. A basic example of building a belief-space roadmap in an environment with ranging beacons. (a) Distribution means are sampled, and the means in C free are kept. (b) Edges between distributions that lie in C free are added to the graph. (c) Once the graph is built, an initial belief (lower right, labelled START) can be propagated through the graph by simulating the agent s motion and sensor measurements, and performing the appropriate filter update steps in sequence along edges. The posterior distribution at each node is drawn with 1 σ uncertainty ellipses, and results from a single-source, minimum uncertainty search path to that node. In this example, we artificially increased the noise in the robot motion to make the positional uncertainty clearly visible throughout the environment. Figure (d) reiterates the benefit of incorporating the full belief distribution in planning. The belief space planner detours from the shortest path through an sensing-rich portion of the environment to remain well-localized. 7

8 Factoring the Covariance The robot has control over its mean, but not the covariance. We would like to efficiently calculate the cost (in terms of the covariance) of each step in the graph. Rather than computer the new covariance for each control step, calculate one transformation matrix from (μ 0, Σ 0 ) to (μ T, Σ T ) 8

9 Factoring the Covariance Ψ t = [ ] B C t [ ] [ ] W X B = Y Z C t t 1 [ ] [ 0 I 0 G T = I M G [ ] St C G R = 0 G T. t easurement update t S M t = RG T ] t [ I ] 0 M I [ ] B C t 1 Problem: this is susceptible to rounding errors Solution: use Hamiltonian method [ ] of composition, with new operator star t [ ] S t = S C t S M t. This is analogous to taking a bunch of scattering media (like lenses) and stacking them, then describing the result with a single equation. 9

10 In other words... ] S 1:T = [ ] G1:T R 1:T M 1:T G T 1:T = S 1 S 2 S T [ ΣT ] = [ ] I Σ0 S 0 I 1:T. To get from point a to point b, use the star operation to compose the matrices for all the time steps necessary to get from a to b. Then a new starting condition can be applied using the star composition of the starting condition and the previously calculated composed transformation matrix. 10

11 The Algorithm Algorithm 1 The Belief Roadmap Build Process. Require: Map C over mean robot poses 1: Sample mean poses {µ i } from C free using a standard PRM sampling strategy to build belief graph node set {n i } such that n i [µ] =µ i 2: Create edge set {e ij } between nodes (n i,n j ) if the straight-line path between (n i [µ],n j [µ]) is collision-free 3: Build one-step transfer functions {ζ ij } e ij {e ij } 4: return Belief graph G = {{n i }, {e ij }, {ζ ij }} 11

12 The Algorithm(s) Algorithm 2 The Belief Roadmap Search Process. Require: Start belief (µ 0, Σ 0 ),goallocationµ goal and belief graph G Ensure: Path p from µ 0 to µ goal with minimum goal covariance Σ goal. 1: Append G with nodes {n 0,n goal }, edges {{e 0,j }, {e i,goal }}, and one-step transfer functions {{ζ 0,j }, {ζ i,goal }} 2: Augment node structure with best path p= and covariance Σ=, suchthatn i ={µ, Σ,p} 3: Create search queue with initial position and covariance Q n 0 ={µ 0, Σ 0, } 4: while Q is not empty do 5: Pop n Q 6: if n = n goal then 7: Continue 8: end if 9: for all n such that e n,n and not n n[p] do 10: Compute one-step update Ψ = ζ n,n ] Ψ, whereψ = [ n[σ] I 1 11: Σ Ψ 11 Ψ 21 12: if tr(σ ) <tr(n [Σ]) then 13: n {n [µ], Σ,n[p] {n }} 14: Push n Q 15: end if 16: end for 17: end while 18: return n goal [p] Algorithm 3 The Min-Max Belief Roadmap (minmax-brm) algorithm. Require: Start belief (µ 0, Σ 0 ),goallocationµ goal and belief graph G Ensure: Path p from µ 0 to µ goal with minimum maximum covariance. 1: G = {{n i }, {e ij }, {S ij }} BUILD BRM GRAPH (B µ ) 2: Append G with nodes {n 0,n goal }, edges {{e 0,j }, {e i,goal }}, and one-step descriptors {{S 0,j }, {S i,goal }} 3: Augment node structure with best path p = and maximum covariance Σ p max = along path p, suchthat n i = {µ, Σ,p,Σ p max} 4: Create search queue with initial position and covariance Q n 0 = {µ 0, Σ 0,, } 5: while Q is not empty do 6: Pop n Q 7: if n = n goal then 8: Continue 9: end if 10: for all n such that e n,n and n n[p] do 11: Compute one-step update Ψ = ζ n,n ] Ψ, whereψ = [ n[σ] I 1 12: Σ Ψ 11 Ψ 21 13: if max(tr(σ ),tr(n[σ p max])) <tr(n [Σ p max]) then 14: n {n [µ], Σ, {n[p],n },max(σ,n[σ p max])} 15: Push n Q 16: end if 17: end for 18: end while 19: return n goal [p] 12

13 Experimental Validation 12 True Distance vs. Range Bias Error in LOS Scenario Ultra-wide bandwidth radio beacons Can be modeled with Gaussian noise In simulation, robot navigating through a small, obstacle-free environment with sensors distributed along a randomized trajectory in the space. Bias Error: Mean and Standard Deviation (m) UWB Ranges Received True Distance (m) 13

14 Results 10 Sensor Model Uncertainty vs. Positional Error at Goal Location 9 Maximum Sensor Range vs. Positional Error at Goal Location 9 BRM Planner 8 Positional Error at Goal Location (m) PRM Planner Positional Error at Goal Location (m) BRM Planner PRM Planner Sensor Noise Standard Deviation (m) Maximum Sensor Range (m) 14

15 Search Tim 10 1 Experiment Search Time (s) Search Time vs. Search Tree Depth Search with Standard EKF Updates Search with One Step EKF Updates Search Tree Depth Search Time (s) Search Tree Depth (a) Time vs. Tree Depth Search Time vs. Path Length GOAL Fig. 7 ( 70 (lowe the r in co positi ellips accu unce Search with Standard EKF Updates for l 10 0 Search with One Step EKF Updates route regio 10 1 BRM 10 2 Fi beacon ning START PRM path BRM path from! ellipse 10 3 throu Path Length (m) posit Fig. 7. Example trajectories for a mobile robot in an indoor environment ( 70m across) with ranging beacons. The robot navigates from START local (lower left) to GOAL (top). (b) Time The vs. BRM Pathfinds Length a path in close proximity to varie the ranging beacons, balancing the shorter route computed by the PRM in configuration space against a lower cost path in information space. The of th Fig. 6. Algorithmic Performance. (a) Time to Plan vs. Tree Depth (b) Time positional uncertainty over the two paths is shown as the bold covariance path to Plan vs. Path Length. Note that these graphs are semi-log graphs, indicating ellipses. two orders of magnitude increase in speed. bloc direc for localization. The BRM planner detours from the direct grow areroute obtained chosen when by comparing the shortestthe path search planner times forwith sensor-rich respect 15 In co to regions the length of theof environment. the resulting Whereas path, the shown shortest in Figure path planner 6(b), local

16 Sample Trajectories START GOAL beacon PRM path BRM path! ellipse Fig. 7. Example trajectories for a mobile robot in an indoor environment ( 70m across) with ranging beacons. The robot navigates from START (lower left) to GOAL (top). The BRM finds a path in close proximity to the ranging beacons, balancing the shorter route computed by the PRM in configuration space against a lower cost path in information space. The positional uncertainty over the two paths is shown as the bold covariance (b) BRM: Lowest Expected Uncertainty Path ellipses. Fig. 8. Example paths for a mobile robot navigating across MIT campus. The solid line in each case is the robot path, the small dots are the range beacons being used for localization, and the dark ellipses are the covariances for localization. The BRM planner detours from the direct (a) PRM: Shortest Path Kaelb nin Art Kaila the pro Kalm filt AS Kavra (19 dim Ro Koen nin ter Pro LaVa pla 378 Lozan spa 108 Missi 16 roa IEE tio