FPGA-Based Real-Time SLAM 2019-05-29آ  FPGA-Based Real-Time SLAM A Major Qualifying Project Report Submitted

Embed Size (px)

Text of FPGA-Based Real-Time SLAM 2019-05-29آ  FPGA-Based Real-Time SLAM A Major Qualifying Project Report...

  • FPGA-Based Real-Time SLAM

    A Major Qualifying Project Report Submitted to the Faculty of

    Worcester Polytechnic Institute

    In partial fulfillment of the requirements for the

    Degree of Bachelor of Science in Electrical & Computer Engineering

    By:

    Georges Gauthier

    John DeCusati

    January 23, 2017

    Advisor:

    Professor R. James Duckworth

  • Contents

    Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . i

    Executive Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii

    1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

    2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

    2.1 Remote Situational Awareness Products . . . . . . . . . . . . . . . . . . . . 3

    2.2 Simultaneous Localization and Mapping . . . . . . . . . . . . . . . . . . . . 5

    2.3 The Zynq Evaluation and Development Board (ZedBoard) . . . . . . . . . . 7

    3 System Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

    4 System Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

    4.1 Rangefinder Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

    4.1.1 Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

    4.1.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

    4.1.3 Commands . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

    4.2 Rangefinder Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

    4.2.1 Zynq7 Processing System . . . . . . . . . . . . . . . . . . . . . . . . 16

    4.2.2 Designing with the Xilinx SDK . . . . . . . . . . . . . . . . . . . . . 18

    4.3 Rangefinder Data Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

    4.3.1 Programmable Software . . . . . . . . . . . . . . . . . . . . . . . . . 20

    4.3.2 Programmable Logic . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

    4.4 PS-PL Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

    4.4.1 Advanced eXtensible Interface (AXI) . . . . . . . . . . . . . . . . . . 24

    4.4.2 Creating Custom IP . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

    4.5 Digital Compass Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

  • 4.5.1 Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

    4.5.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

    4.5.3 Register Settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

    4.5.4 Read Registers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

    4.6 Compass Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

    4.6.1 Re-Customizing the Zynq7 Processing System . . . . . . . . . . . . . 29

    4.7 Compass Data Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

    4.7.1 Programmable Software . . . . . . . . . . . . . . . . . . . . . . . . . 30

    4.8 Camera Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

    4.8.1 Camera Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

    4.8.2 Camera Signaling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

    4.8.3 I2C Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

    4.8.4 Image Buffering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

    4.9 Disparity Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

    4.9.1 Image Rectification . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

    4.9.2 Sum of Absolute Differences . . . . . . . . . . . . . . . . . . . . . . . 41

    4.10 Combined Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

    4.10.1 Rangefinder and Disparity Data Integration . . . . . . . . . . . . . . 44

    4.10.2 Accounting for Compass Data . . . . . . . . . . . . . . . . . . . . . . 45

    5 Testing and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

    5.1 Rangefinder Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

    5.1.1 Testing via the Data Viewing Tool . . . . . . . . . . . . . . . . . . . 46

    5.1.2 Command Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

    5.1.3 Communication via USB On-The-Go (OTG) . . . . . . . . . . . . . . 49

    5.1.4 Communication via Pmod . . . . . . . . . . . . . . . . . . . . . . . . 50

    5.1.5 PS-PL Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

    5.1.6 Data Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

  • 5.2 IMU Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

    5.2.1 Communication Testing . . . . . . . . . . . . . . . . . . . . . . . . . 59

    5.2.2 Data Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

    5.2.3 Interfacing with the ADIS16375 IMU . . . . . . . . . . . . . . . . . . 63

    5.3 Single Camera Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

    5.3.1 I2C Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

    5.3.2 Data Management . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

    5.3.3 Transmitting Images Over UART for Analysis . . . . . . . . . . . . . 69

    5.4 Final Camera Hardware Implementation . . . . . . . . . . . . . . . . . . . . 72

    5.4.1 Stereo Camera Breakout Board . . . . . . . . . . . . . . . . . . . . . 72

    5.4.2 Image Buffering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

    5.4.3 Resource Management . . . . . . . . . . . . . . . . . . . . . . . . . . 78

    5.5 Disparity Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

    5.5.1 Image Rectification . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

    5.5.2 Matlab Implementation . . . . . . . . . . . . . . . . . . . . . . . . 80

    5.5.3 Verilog Test Bench . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

    5.5.4 Test Bench Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84

    5.5.5 Final Disparity Implementation . . . . . . . . . . . . . . . . . . . . . 87

    5.6 Full System Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

    6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97

    6.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

    References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

    Appendix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

    A Component Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

    B Camera Module Control Register . . . . . . . . . . . . . . . . . . . . . . . . 103

    C Stereo Camera Schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

  • D Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

    D.i MT9V034 and Al422b Test Code . . . . . . . . . . . . . . . . . . . . 105

    D.ii Matlab Disparity Algorithm Implementation . . . . . . . . . . . . . 113

    D.iii Custom IP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116

    D.iv Programmable Software . . . . . . . . . . . . . . . . . . . . . . . . . 148

    D.v LUT Initialization Code for Transformation from Polar to Cartesian . 154

  • List of Figures

    1 Robotic Situational Awareness Devices . . . . . . . . . . . . . . . . . . . . . . . . . 1

    2 Serveball’s Squito [3] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

    3 Serveball’s Squito Input and Output [3] . . . . . . . . . . . . . . . . . . . . . . . . . 4

    4 Real-Time SLAM with a Single Camera [4] . . . . . . . . . . . . . . . . . . . . . . . 5

    5 From Left to Right: Original Image, Disparity Map, Object Detection Results [5] . 6

    6 The Avnet ZedBoard [6] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

    7 ZedBoard Block Diagram [7] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

    8 System Block Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

    9 URG-04LX Scanning Laser Rangefinder [9] . . . . . . . . . . . . . . . . . . . . . . . 12

    10 Timing Diagram of RS-232 (top) and TTL Communication Protocols [11] . . . . . . 13

    11 RS-232 to TTL Converter with RS-232 Breakout Board . . . . . . . . . . . . . . . . 13

    12 Top-Down View of Rangefinder Field of Vision [12] . . . . . . . . . . . . . . . . . . 15

    13 Zynq7 Processing System Customization Window [13] . . . . . . . . . . . . . . . . . 16

    14 Zynq7 Processing System PS-PL Configuration Window . . . . . . . . . . . . . . . . 17

    15 Generating a Bitstream in Vivado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

    16 Creating a New Application Project in the Xilinx SDK . . . . . . . . . . . . . . . . 19

    17 The PmodNAV 10-Axis IMU [18] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

    18 Magnetometer SPI Read and Write Protocol [16] . . . . . . . . . . . . . . . . . . . . 27

    19 Magnetometer Multip

Recommended

View more >