Planning in Information Space for a Quadrotor Helicopter

Planning in Information Space for a Quad-rotor Helicopter Ruijie He Thesis Advisor: Prof. Nicholas Roy

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Introduction Goal: Indoor Autonomous Helicopter

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Motion Capture System ~$150, 000 Goal: Helicopter that navigates autonomously in ANY indoor environment

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Introduction l Goal: Helicopter that navigates autonomously in any indoor Sick laser rangefinder environment l l l No GPS Incomplete state information Range: 80 m Weight: 4. 5 kg FOV: 180˚ Localize via Onboard Sensors l Challenges: Sonar sensors Range: 5. 8 m Weight: 0. 017 kg FOV: 15˚ Noisy measurements Limited payload (< 500 g) Lightweight sensors l Noisy measurements l Limited Field-of-view l Limited Range l l Camera sensors Range: N. A. Weight: 0. 030 kg FOV: 70˚ Computationally-intense Hokuyo laser rangefinder Range: 4 m Weight: 0. 160 kg FOV: 240˚

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Introduction l Goal: Helicopter that navigates autonomously in any indoor environment l l l No GPS Incomplete state information Localize via Onboard Sensors l Challenges: Limited payload (< 500 g) Lightweight sensors l Noisy measurements l Limited Field-of-view l Limited Range l l Sensor chosen: Hokuyo Laser Hokuyo laser rangefinder Limitation: Range: 4 m Weight: 0. 160 kg 4 m Range FOV: 240˚

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Project Overview l l Conference Paper accepted for ICRA’ 08 (Pasedena, CA) Problem statement l l l Background l l l Traditional Path-planning Belief Roadmap Algorithm (BRM) Contributions l l l Develop Path-planning algorithms that account for sensor limitations, e. g. Limited range Active Localization Extending BRM to UKF BRM Sampling Strategy Conclusion

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Traditional Path-Planning l Assumption: l l l Full State of Vehicle Known Probabilistic Roadmap Algorithm A* Search Goal: Shortest Path from Start to Goal

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap BRM Sampling Traditional Path-Planning l l l Incomplete state information Incorporates Uncertainty Maintain belief state: Conclusion

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Incorporating Uncertainty in Path-planning l Actual helicopter tests l l Planned path using different algorithms Fly path via RC, collect laser log Post-processed laser log for localization Can localizer trace true helicopter path? Traditional path-planning Shortest no-collision path Result: Failed localization Accounting for Sensor limitations Seeks to minimize sensor uncertainty at goal Result: Successful localization

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Belief Roadmap Algorithm l l l Prentice, Roy. The Belief Roadmap: Efficient Planning in Linear POMDPs by Factoring the Covariance. (ISRR, 2007) Goal: Minimize Uncertainty at Goal Step 1: Sample points in Cfree space Step 2: Connect edges if collision-free Step 3: Build transfer function for each edge

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Belief Roadmap Algorithm l Edge Construction Initial Conditions Different Initial Conditions ? = Jacobian of control model = Control noise = Information gain l Assumes EKF localization

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Contribution 1 Extending BRM to UKF

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Extending BRM to UKF l Existing BRM assumes EKF update l Great for landmark measurements 2 1 3 4 5

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Extending BRM to UKF l Existing BRM assumes EKF update l Great for landmark measurements Range, d θ d Bearing, θ Linearization used for calculating Jacobians y y

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Extending BRM to UKF l Existing BRM assumes EKF update l l Great for landmark measurements Non-point features? E. g. Walls Discretize wall into landmarks Problems: l l Data association Non-independence of landmarks Assuming landmark independence in EKF Range θ True measurement & Jacobian ? Range ? ? y y

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Extending BRM to UKF l Unscented Kalman Filter [Julier et. al 1995] l l l Eliminates the need for linearizations and Jacobians Uses a set of 2 n+1 sigma points to represent probability density Accurate up to 2 nd order

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Extending BRM to UKF l Goal: Calculate transfer function l In EKF, Mt is the information gain due to measurement zt l Key contribution: l For UKF, = Info. Matrix after measurement update = Est. Info. Matrix after control update = Predicted covariance after control update = Kalman gain = Measurement uncertainty matrix

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling BRM UKF Results Comparison of one-step BRM UKF vs. full UKF covariance Trace of BRM Covariance l l BRM-UKF approximates UKF update l Performed over range of motions and randomized initial conditions l Result l l Trace of Covariance using Normal UKF updates Trace of covariances closely matched Errors induced from approximation are very low

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap BRM Sampling Contribution 2 Sensor Uncertainty Sampling Conclusion

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling in Belief Space l BRM samples from Cfree, similar to PRM l More samples -> Better paths l Graph search – exponential time l Optimal path vs. Efficient Search Solution: Generate samples at positions that maximize localization accuracy of vehicle

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Sensor Uncertainty Field* Low Information Gain * Takeda, Latombe, Sensory uncertainty field for mobile robot navigation, ICRA 1992 High Information Gain

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap “Sensor Uncertainty”(SU) Sampling l Conclusion BRM Sampling Expected info. gain from given map = Prior entropy – Posterior entropy l l Low High Information gain: Information gain used to probabilistically accept/reject sample

Introduction Traditional Path-plan Extending BRM to UKF Sensor Uncertainty Sampling Project Overview l l Belief Roadmap Conclusion BRM Sampling No need to create entire SUF Uniform vs. Sensor Uncertainty Sampling Uniform Sampling Sensor Uncertainty Sampling

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Results – Uniform vs. SU Sampling Conclusion BRM Sampling l Compared sampling strategies l BRM w/ Uniform Sampling l BRM w/ SU Sampling l Results l SU sampling – Smaller covariances l Takes longer time to build, but better paths

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Hovering with Laser BRM Sampling Conclusion

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap Conclusion BRM Sampling Summary l l Goal: Autonomous navigation and path-planning without complete state information Contributions: l l Extended Belief Roadmap to UKF localization Efficient sampling strategy

Backup slides

Other Research l Design of Nano-Aerial Vehicle l l l 16. 62 x project United Technologies Corporation award 1 st place, 2007 AIAA Region I-NE Student Conference (Undergraduate division) Presented at 2008 AIAA Int’l Student Conference MAV 08 Competition (Ongoing) l l Autonomous reconnaissance and rescue mission Vision SLAM, dynamic path-planning, autonomous control

Future Work l Fully autonomous indoor helicopter l l Sensor limitations l l Localization – UKF-PF hybrid Planning in 6 dof Integrating IMU sensors Extending to camera sensors Laser 3 D SLAM Camera sensors Range: N. A. Weight: 0. 030 kg FOV: 70˚ Computationally-intense

Hardware l X 3 D-BL Quadrotor Helicopter l l l Ascending Technologies Max 500 g payload 15 min. flight time 55 cm max. dimensions Gyro-stabilization in all axes Hokuyo URG-04 LX Laser Sensor l l l 240˚ Field of View 4 m Max range 10 Hz update

Extended Kalman Filter l State st & observation zt models l Process step l Measurement step l l Kalman gain Information form

Unscented Kalman Filter l Algorithm l Generate “sigma points” l Propagate samples, generate process mean & covariance l Measurement step: l Simulate measurement at sigma pts

Belief Roadmap Algorithm (1) l Problem: Edge Construction Initial Conditions Different Initial Conditions ? l Need to perform simulation for multiple updates along each edge, for every start state l Computing minimum cost path of 30 edges: ≈100 seconds

Belief Roadmap Algorithm (3) l EKF Covariance Update Control: Measurement: l Factor Covariance matrix: l Control update: Measurement update:

Belief Roadmap Algorithm l Transfer function: l One-step covariance update Reduces belief-space planning search complexity to approx. configuration space planning

BRM UKF Results l Distribution of errors using constant prior assumption l l UKF depends on prior matrix Different priors may result in different one-step transfer functions Performed test with 100 different priors and calculated error in trace of covariance Result l l Error in trace of covariance is less than 2% with significance of p = 0. 955 Low sensitivity to choice of prior

Bayes filter

“Sensor Uncertainty”(SU) Sampling l Expected info. gain from given map l l l Low High Diff. in posterior and prior entropy High Information gain: Information gain used to probabilistically accept/reject sample

Background - MDPs l States, Actions, State transition functions, Rewards, Policy Goal: Determine optimal policy (Set of state-action pairs) at each state that gives highest value function Value function: Rewards over infinite horizon/ finite horizon t Bellman equation: Given a policy, Value function = l Value Iteration algorithm: l l l

Background - POMDPs l l l Imprecise observations – No certainty of which states you’re currently in Could use max. likelihood, but loses a lot of info Uses belief states – probabilistic representation of states l l No. of dimensions = no. of “real” states - 1 Calculating Value function l l l Represented by α-vectors Eg. 2 states, 2 actions, 3 observations Immediate rewards Value function = Γa, * Set of α-vectors for taking action a over all states α Vector describing value function

Background – POMDPs (2) l Given fixed action (a 1) & observation: l Transformed value function

Background – POMDPs (3) l Transformed value function for all observations: l Partition for action a 1:

Background – POMDPs (4) l Value function and partition for action a 1: l (Combined a 1, a 2)/(Horizon 2) value function

Background – POMDPs (5) l l l Exact value update created optimal policy at every belief state Space complexity issues Use approximate techniques – PBVI l Γt Set of α-vectors at finite time T A Set of actions Z Set of observations Trades off computation time with solution quality

Probabilistic Roadmap

Belief Roadmap Algorithm

Planning in Belief Space l l No perfect information, only state estimate b = {μ, Σ} Account for uncertainty information {Σt} l l Covariance must be propagated throughout proposed paths to test desirability l l l Beliefs with high uncertainty must be avoided Common state estimators include Kalman Filter (KF), Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF) Requires multiple EKF updates in a path Each initial covariance must be propagated through these updates – not a one-time cost.

Belief Roadmap Algorithm l l l Sam Prentice and Nicholas Roy, 2007 Allows multiple EKF updates to be compiled into a single linear transfer function between two mean poses {μi, μj} Reduces belief space planning search complexity to level comparable with configuration space planning

Belief Roadmap Algorithm l Basic Algorithm: 1. Sample mean poses {μi} from Cfree using standard PRM sampling strategy to build graph of mean nodes {ni} 2. Create edge set {eij} between nodes {ni, nj} if straight-line path is collision-free 3. Build one-step transfer function {ξij} for all edges eij 4. Perform Breadth-first search with initial {μ 0, Σ 0} and goal {μgoal} to find path that minimizes uncertainty cost

Extending BRM to UKF l EKF performs poorly when linearizing control, measuring functions leads to poor approximations l l E. g. Localization in discrete, grid-based maps using measurements Grid cells – strong independence assumption Requires high-level feature extraction Unscented Kalman Filter [Julier et. al 1995] l l Uses a set of 2 n+1 sigma points to represent probability density Eliminates the need for linearization l l l Distribution accurate up to 2 nd order Unscented Transform computes moments of process and measurement distributions directly However, for BRM, Mt matrix is no longer computed

Extending BRM to UKF (Math) l Goal: Calculate transfer function Need to recover l In EKF, Mt is the information gain due to measurement zt l UKF Covariance update does not depend on actual measurement l

Introduction Traditional Path-plan Project Overview Extending BRM to UKF Belief Roadmap “Sensor Uncertainty”(SU) Sampling l l Low High BRM Sampling Expected info. gain from given map l l Conclusion Diff. in posterior and prior entropy High Information gain: Information gain used to probabilistically accept/reject sample