Use of Vision Sensors and Lane Maps to Aid GPS-INS Navigation by John W Allen A thesis submitted to the Graduate Faculty of Auburn University in partial fulfillment of the requirements for the Degree of Master of Science Auburn, Alabama December 12, 2011 Keywords: Sensor Fusion, Kalman Filter, GNSS Copyright 2011 by John W Allen Approved by David M. Bevly, Chair, Associate Professor of Mechanical Engineering David Beale, Professor of Mechanical Engineering Andrew J. Sinclair, Associate Professor of Aerospace Engineering Abstract This thesis proposes a method to increase observability of a GPS/INS system operating under limited satellite coverage. Measurements from vision sensors are used to supplement the GPS pseudorange and pseudorange-rate measurements. Both LiDAR and camera mea- surements are used to measure the lateral position of a vehicle in its current lane. The vision measurementsprovidelocalbasedpositioningbasedoffthelane. Amapofthelaneisusedto relatethelocalpositionmeasurementprovidedbythevisionsystemstotheglobalcoordinate frame used by GPS and the navigation filter. Since the filter is used for ground vehicles, the height above the ground is constant. The constant height can be used to constrain position in another axis that is orthogonal to the axis in which lane position measurements are given. In order to test the performance of the navigation filter, real data from the NCAT test track in Opelika, Alabama will be used. RTK GPS is used as a “truth” metric in order to determine filter performance. Experimental results show that using vision measurements with a precise lane map results in centimeter level global accuracy in the axis perpendicular to the road and meter level (depending on the quality of GPS measurements available) global accuracy in the axis parallel with the road. It is also shown that the navigation filter remains observable and functional if two GPS observations along with vision measurement and a lane map are available. The results also show a reduction in position drift when GPS is unavailable if vision measurements and a lane map are available. ii Acknowledgments I would like to thank god and my family without whom none of this would be possible. I would like to thank Jordan Britt and Christopher Rose for their support in this project. I would like to thank all the members of the GAVLAB for their help. I would like to thank my advisor, Dr David Bevly. I would also like the thank the Federal Highway Administration for funding this research. iii Table of Contents Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vi List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1 GPS Based Navigation Filter . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Incorporating Vision Measurments . . . . . . . . . . . . . . . . . . . . . . . 4 1.3 Thesis Contribution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.1 Inertial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.1.1 Crossbow IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2 Multi-sensor Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.2.1 Discrete Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . 15 2.3 Global Positioning System . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3.1 Measurement Structure . . . . . . . . . . . . . . . . . . . . . . . . . . 19 2.3.2 Septentrio GPS Receiver . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.4 Lane Positioning Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.4.1 LiDAR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.4.2 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 3 Six Degree of Freedom Navigation Filter . . . . . . . . . . . . . . . . . . . . . . 28 3.1 States and State Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.2 Time Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.3 Measurement Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 iv 3.3.1 Loosely Coupled GPS Measurement Update . . . . . . . . . . . . . . 39 3.3.2 Closely Coupled GPS Measurement Update . . . . . . . . . . . . . . 40 3.4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 4 Vision Measurement and Vehicle Constraint Updates Using a Map . . . . . . . . 48 4.1 Waypoint Based Lane Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 4.1.1 Determining Waypoint Position . . . . . . . . . . . . . . . . . . . . . 51 4.1.2 Determining Euler Angles Between Global Axis and Road . . . . . . 53 4.1.3 Determining Distance Between Waypoints . . . . . . . . . . . . . . . 56 4.2 Vision Measurement Update . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 4.2.1 Vision Measurement Model . . . . . . . . . . . . . . . . . . . . . . . 56 4.2.2 Map Keeping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 4.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63 5.1 Effects of Sensor Failures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.2 Limited Satellite Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 6.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 Appendices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 A Coordinate Frame Rotation and Translation . . . . . . . . . . . . . . . . . . . . 94 A.1 Two-Dimensional Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . 94 A.2 Three-Dimensional Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . 95 A.3 Coordinate Frame Translation . . . . . . . . . . . . . . . . . . . . . . . . . . 96 A.4 Global Coordinate Frame Rotations . . . . . . . . . . . . . . . . . . . . . . . 97 B Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 C Table of Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 v List of Figures 1.1 Loosely Coupled Filter Architecture . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Closely Coupled Filter Architecture . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.3 Filter Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2.1 Crossbow 440 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.2 Single Axis Position Estimation without Measurement Noise . . . . . . . . . . . 14 2.3 Single Axis Position Estimation with Measurement Noise . . . . . . . . . . . . . 15 2.4 GPS Measurement Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.5 Septentrio GPS Receiver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.6 Laser Scanner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.7 Average echo width for 100 scans [1] . . . . . . . . . . . . . . . . . . . . . . . . 24 2.8 Corresponding echo width to lane markings [1] . . . . . . . . . . . . . . . . . . 25 2.9 Lane lines extracted from the Hough transform [2] . . . . . . . . . . . . . . . . 26 3.1 Earth Centered Earth Fixed Coordinate Frame . . . . . . . . . . . . . . . . . . 29 3.2 Vehicle Body Coordinate Frame [3] . . . . . . . . . . . . . . . . . . . . . . . . . 30 4.1 Road Coordinate Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 vi 4.2 Waypoint Baselines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 5.1 Estimated Vehicle Position Using Loosely Coupled RTK GPS Updates . . . . . 63 5.2 Estimated Vehicle Position Using Closely Coupled GPS Updates . . . . . . . . . 65 5.3 Estimated Vehicle Pitch and Roll . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.4 Estimated Vehicle Heading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 5.5 Estimated Velocity with Measurement Failures . . . . . . . . . . . . . . . . . . 68 5.6 Estimated Lane Position with Measurement Failures . . . . . . . . . . . . . . . 69 5.7 Estimated Lane Position Error with Measurement Failures . . . . . . . . . . . . 70 5.8 Estimated Longitudinal Position with Measurement Failures . . . . . . . . . . . 71 5.9 Estimated Longitudinal Position Error with Measurement Failures . . . . . . . . 72 5.10 Estimated Position with Measurement Failures . . . . . . . . . . . . . . . . . . 73 5.11 Estimated Errors for Various Map and Vision Availability Using No GPS . . . . 74 5.12 Satellite Line of Sight Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 5.13 Estimated Velocity with Vision and Limited Satellite Visibility . . . . . . . . . 77 5.14 Estimated Lane Position with Vision and Limited Satellite Visibility . . . . . . 78 5.15 Estimated Longitudinal Position with Vision and Limited Satellite Visibility . . 79 5.16 Estimated Longitudinal Position Error with Vision and Limited Satellite Visibility 80 5.17 Estimated North-East Position with Vision Measurements, Map, and Limited Satellite Visibility . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81 vii 5.18 Estimated North-East Position with Map and Limited Satellite Visibility . . . . 82 5.19 Estimated North-East Position with Limited Satellite Visibility . . . . . . . . . 83 5.20 Receiver Clock Bias Error for Various Satellite Combinations . . . . . . . . . . 84 5.21 Estimated Longitudinal Position for All Possible SV Pairs . . . . . . . . . . . . 85 5.22 Estimated Longitudinal Error for All Possible SV Pairs . . . . . . . . . . . . . . 86 A.1 Two-Dimensional Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 A.2 Three-Dimensional Rotation Sequence . . . . . . . . . . . . . . . . . . . . . . . 95 A.3 Two-Dimensional Coordinate Frame Translation and Rotation . . . . . . . . . . 96 A.4 North-East-Down Coordinate Frame . . . . . . . . . . . . . . . . . . . . . . . . 97 B.1 Filter Observability with Respect to Time . . . . . . . . . . . . . . . . . . . . . 100 B.2 Filter Observability (Full System) (GPS Outage) . . . . . . . . . . . . . . . . . 101 B.3 Filter Observability (Full System) (2 GPS Observations during Outage) . . . . . 102 viii List of Tables 3.1 Diagonal Elements of the Process Noise Covariance Matrix . . . . . . . . . . . . 36 3.2 Tracking Loop Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 5.1 Satellite Azimuth and Elevation Angles . . . . . . . . . . . . . . . . . . . . . . . 75 5.2 Longitudinal Position Error after 30 Seconds for Every SV Pair . . . . . . . . . 87 C.1 Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 C.2 Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 ix Chapter 1 Introduction In order to reduce the number of traffic fatalities that occur due to unintentional lane departures, many vehicle manufacturers are developing lane departure warning (LDW) sys- tems. LDW systems alert the driver before the vehicle departs the lane. Most of the LDW systems in production now are solely based off camera measurements. A LDW camera uses feature extraction to determine lateral position in the current lane. The feature used for a camera-based LDW system is the painted lane lines. A single camera can not provide three-dimensional ranging information due to the unresolved distance from the camera to the feature of interest; although the camera can provide lateral position in a lane without re- solvingthisdistance. Camera-basedLDWsystemsarepronetofailuresduetoroad, weather, and lighting conditions. A more in-depth look at how the camera is used to determine lane position can be found in [2]. Current research is underway to provide robustness to current camera-based LDW sys- tem by adding other types of sensors. A LiDAR (Light Detection And Ranging) is an active type of vision sensor. A LiDAR works like sonar; however, instead of using sound waves, a LiDAR uses light waves to provide ranging. Unlike the camera, a LiDAR can provide three-dimensional ranging information. One drawback to the LiDAR is feature extraction can be more difficult than feature extraction using a camera. To overcome this difficulty, the LiDAR also provides reflectivity data. For the LDW case, a painted lane marker has a different reflectivity than the asphalt around it. The reflectivity data can be used to extract lane markers, and the scan angle information can be used to provide a measurement of the lateral position of the vehicle in the lane. A more in-depth look at how the LiDAR is used to determine lane position can be found in [1]. 1