Report

SLAM Accelerated Using Hardware to improve SLAM algorithm performance Project Overview Team Members Roy Lycke Ji Li Ryan Hamor Take existing SLAM algorithm and implement on computer Analyze Performance of algorithm to determine kernels to be accelerated in HW Implement SLAM algorithm on PowerPC with previously identified kernels in HW RH What is SLAM? SLAM stands for Simultaneous Localization and Mapping Predict pose using previous and current data Types of pose sensors Wheel Encoders GPS Detect landmarks and correlated to robot using predicted pose. Types of Observation Sensors Sonar Infrared Laser Scanners Video RH Current State of SLAM Algorithms SLAM algorithms fall into two main categories Extend Kalman Filter Large Covariance Matrix to Process Particle Filter Each Particle contains pose estimate and map RH Particle Filter Algorithm RH What we have Decided to do Started with existing SLAM implementation ratbot-slam developed by Kris Beevers ratbot-slam Uses particle filter algorithm and multiple observation scans using just wheel encoders and 5 IR sensors We modified ratbot-slam to use log files taken from radish.sourceforge.net RH Ratbot-slam Modifications Create new observation function using laser scans vs. original IR sensors. Modify motion model to use dead-reckoned odometry RH Demo of Modified ratbotslam RH Profile of Modified Code RL Areas that can be Accelerated Decided to accelerate predict step included: motion_model_deadreck gaussian_pose Estimated Maximum speed up 39% or 1.64x Why not squared_distance_point_segment? Least understood of algorithms we could accelerate If we had more time we would have developed this RL Function Acceleration Design Decisions Fixed or Floating Point? Fixed point Implementation done in fixed point Resources required to do floating point were significantly heavier Heavily Pipeline or Create Predict Stage for each particle? Heavily Pipelined Data is serially loaded through load and save function to coprocessor It would take too many resources to implement predict stages in parallel for each particle RL Top Level Design RL Motion Model C-Code RH MotionModel Data Flow RH MotionModel Data Flow RH MotionModel HDL Stats RH Gaussian Pose void gaussian_pose(const pose_t *mean, const cov3_t *cov, pose_t *sample) { sample->x = gaussian(mean->x, fp_sqrt(cov->xx)); sample->y = gaussian(mean->y, fp_sqrt(cov->yy)); sample->t = gaussian(mean->t, fp_sqrt(cov->tt)); } JL Gaussian Pose fixed_t gaussian(fixed_t mean, fixed_t stddev) { static int cached = 0; static fixed_t extra; static fixed_t a, b, c, t; if(cached) { cached = 0; return fp_mul(extra, stddev) + mean; } // pick random point in unit circle do { a = fp_mul(fp_2, fp_rand_0_1()) - fp_1; b = fp_mul(fp_2, fp_rand_0_1()) - fp_1; c = fp_mul(a,a) + fp_mul(b,b); } while(c > fp_1 || c == 0); t = pgm_read_fixed(&unit_gaussian_table[c >> unit_gaussian_shift]); extra = fp_mul(t, a); cached = 1; return fp_mul(fp_mul(t, b), stddev) + mean; } JL Parallelism & Acceleration Techniques Parallelism gaussian_pose function is consists of three gaussian functions. gaussian functions can be separated into two parts Acceleration TechniquesPipelineMulti-thread JL Top Level Diagram of gaussian_Pose JL Random Number Generator Xorshift random number generators are developed. They generate the next number in their sequence by repeatedly taking the exclusive or (XOR) of a number with a bit shifted version of itself. JL Random_Number_Manager JL Gaussian Entity JL Demo of FPGA System RL Timing Analysis of Original System Timing analysis was performed via run-time clock counts and print statements to the minicom Sections of code timed include: Predict Step, Multiscan Feature Extraction and Data Association Step, & Filter Health Evaluation and Re-sample Step The Predict Step was implemented on the FPGA for acceleration Initial timing analysis : Operation Predict Step - Original Multiscan Step - Original Filter Step - Original Average Runtime Present in (in microseconds) percentage of runs 100% 107,502 2,487,969 2.17% 3,394 2.17% RL Timing Analysis of Accelerated System Timing analysis for accelerated implementation was performed in same manner as original implementation Results shown along with original timing analysis From the data collected, the Predict Step was accelerat ed by 88% Operation Predict Step - Original Multiscan Step - Original Average Runtime (microseconds) 107,502 Present in percentage of runs 100% 2,487,969 2.17% Filter Step - Original 3,394 2.17% Predict Step - Accelerated 12,784 100% 1,982,950 1.94% 13,291 1.94% Multiscan Step - Accelerated Filter Step - Accelerated RL Result Analysis With the Predict Step accelerated by 88.108%, the overall system is accelerated by: 34% = 39% x 88% Result is a reliable and sizable acceleration to the system execution time Analysis of other components Multiscan Step accelerated by 20.29% Filter Step slowed by 74.46% Differences may be due to different values generated by FPGA implementation vs. Original implementation Both implementations use random values More accurate values may lead to longer calculation in other components RL Difficulties with Project Implementation Networking issues Data transfer - differences between PowerPC and Linux Limitations of FPGA Unpredictable execution halting Lack of resource libraries Timing performed with specialized Xilinx library Code needed to be modified to run PC vs. FPGA Environment Output file format is different Issue figuring out how to add multiple files to custom IP RL Conclusions Based on the run-time analysis of our implementation of the accelerated SLAM algorithm there was an appreciable speed up achieved. Our Implementation achieved a speed up of approximately 34% or 1.51x out of an ideal 39% or 1.64x This result shows that if more of the SLAM algorithm was implemented on an FPGA there could be a greater acceleration. Top issue in SLAM implementations is getting algorithm’s implemented on embedded real time systems RH Future Directions Add more regions of the Algorithm to the FPGA acceleration Current implementation only accelerates 39% of system Run SLAM system on different FPGA FPGAs with more robust processors may overcome some of the limitations our implementation faced Run different SLAM algorithm Current implementation is a particle filter algorithm, a Kalman filter algorithm would be next Load data onto board rather than using PC interaction Load data via memory card Perform single data load and perform memory management on the FPGA RL References 1. Durrant-Whyte, Bailey, “Simultaneous Localization and Mapping: Part 1”, IEEE Robotics and Automation Magazine, June 2006, pg 99 – 1082. 2. Durrant-Whyte, Bailey, “Simultaneous Localization and Mapping: Part 2”, IEEE Robotics and Automation Magazine, September 2006, pg 108 - 1173. 3. Bonato, Peron, Wolf, Holanda, Marques, Cardoso, “An FPGA Implementation for a Kalman Filter with Application to Mobile Robotics”, Industrial Embedded Systems, 2007, pg 148 – 1554. 4. Bonato, Marques, Constantinides, “A Floating-point Extended Kalman Filter Implementation for Autonomous Mobile Robots”, Field Programmable Logic and Applications, 2007, pg 576-5795. 5. Beevers K.R., Huang, W.H., “SLAM with Sparse Sensing”, Robotics and Automation 2006, pg 2285-2290 RL Questions? RL