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

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

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

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

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

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

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

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

Incorporating Uncertainty in Path-planning Actual helicopter tests 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 Accounting for Sensor limitations Seeks to minimize sensor uncertainty at goal Result: Failed localization Result: Successful localization Introduction Project Overview Traditional Path-plan Belief Roadmap Extending BRM to UKF BRM Sampling Conclusion

Belief Roadmap Algorithm 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 C free space Step 2: Connect edges if collision-free Step 3: Build transfer function for each edge Introduction Project Overview Traditional Path-plan Belief Roadmap Extending BRM to UKF BRM Sampling Conclusion

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

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

Extending BRM to UKF Existing BRM assumes EKF update Great for landmark measurements Introduction Project Overview Traditional Path-plan Belief Roadmap Extending BRM to UKF BRM Sampling Conclusion

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

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

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

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

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

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

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

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

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

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

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

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

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

Backup slides

Other Research Design of Nano-Aerial Vehicle 16.62x 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 MAV08 Competition (Ongoing) Autonomous reconnaissance and rescue mission Vision SLAM, dynamic path-planning, autonomous control

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

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

Extended Kalman Filter State s t & observation z t models Process step Measurement step Kalman gain Information form Extended Kalman Filter

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

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

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

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

BRM UKF Results Distribution of errors using constant prior assumption 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 Error in trace of covariance is less than 2% with significance of p = Low sensitivity to choice of prior

Bayes filter

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

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 = Value Iteration algorithm: Background - MDPs

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 No. of dimensions = no. of “real” states - 1 Calculating Value function Represented by α-vectors Eg. 2 states, 2 actions, 3 observations Immediate rewards Value function = Background - POMDPs Γ a,* Set of α-vectors for taking action a over all states αVector describing value function for set of actions

Given fixed action (a1) & observation: Transformed value function Background – POMDPs (2)

Transformed value function for all observations: Partition for action a1: Background – POMDPs (3)

Value function and partition for action a1: (Combined a1, a2)/(Horizon 2) value function Background – POMDPs (4)

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

Probabilistic Roadmap

Belief Roadmap Algorithm

Planning in Belief Space No perfect information, only state estimate b = {μ, Σ} Account for uncertainty information {Σ t } Beliefs with high uncertainty must be avoided Covariance must be propagated throughout proposed paths to test desirability 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.

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 } Belief Roadmap Algorithm Reduces belief space planning search complexity to level comparable with configuration space planning

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

Extending BRM to UKF EKF performs poorly when linearizing control, measuring functions leads to poor approximations 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] Uses a set of 2n+1 sigma points to represent probability density Eliminates the need for linearization Distribution accurate up to 2 nd order Unscented Transform computes moments of process and measurement distributions directly However, for BRM, M t matrix is no longer computed

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

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