METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED BY GROUND VEHICLE DYNAMICS Except where reference is made to the work of others, the work described in this thesis is my own or was done in collaboration with my advisory committee. This thesis does not include proprietary or classified information. _______________________________ William E. Travis Certificate of Approval: _____________________________ George Flowers Professor Mechanical Engineering _____________________________ David M. Bevly, Chair Assistant Professor Mechanical Engineering _____________________________ John Y. Hung Associate Professor Electrical Engineering _____________________________ Stephen L. McFarland Dean Graduate School METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED BY GROUND VEHICLE DYNAMICS William E. Travis 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 May 11, 2006 iii METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED BY GROUND VEHICLE DYNAMICS William E. Travis Permission is granted to Auburn University to make copies of this thesis at its discretion, upon request of individuals or institutions and at their expense. The author reserves all publication rights. __________________________________ Signature of Author __________________________________ Date of Graduation iv THESIS ABSTRACT METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED BY GROUND VEHICLE DYNAMICS William E. Travis Master of Science, May 11, 2006 (B.S., Auburn University, 2004) 145 Typed Pages Directed by David M. Bevly A navigation system was designed using an extended Kalman filter for an autonomous ground vehicle competing in the 2005 DARPA Grand Challenge. An overview of this system is provided, and errors in the navigation solution are explained. These errors are attributed to vehicle dynamics unaccounted for in the navigation model. Investigation of these errors begins with the development of a nonlinear simulation to provide vehicle state information in a controlled environment. These vehicle states are used in various navigation models to show difference in navigation accuracy when lateral vehicle dynamics are taken into account. Accuracy with and without GPS measurements is examined. The study then utilizes experimental data to provide similar results. Also, sources of error stemming from typical velocity sensors are explained. v Finally, a method is proposed to utilize a laser scanner to provide measurements for use in the navigation models incorporating lateral vehicle motion. This method could also be used to provide a vehicle controller lateral error from a defined corridor. The method is explained, and experimental results from a simple test bed are shown. vi ACKNOWLEDGEMENTS Without the love and support from others, this thesis would not have been possible. First and foremost I give my thanks to God, who has always met my needs in abundance. He has offered me many blessings throughout life, and always placed the right people in my life at the right time. As I go forward, I continue to pray He uses me to complete His will. The Lord has also provided me with two parents whose love and support molded me into who I am today. They are the perfect role models, exemplifying love, hard work, determination, and good values. I thank them for their many sacrifices that allowed me to pursue my goals and dreams. I hope one day I can give back even a small portion of what you gave to me. The members of the GPS and Vehicle Dynamics Lab, past and present, vastly contributed to my education. I thank you for explanations to the many, many questions I have had. Special thanks go to Rob Daily and Adam Simmons for providing answers to the hardest of my questions. I also owe a special thank you to my advisor, Dr. Dave Bevly, for welcoming me into the lab as an undergraduate. Without his technical and financial support, I would not have had the opportunity to attend graduate school. I thank him for allowing me to work on some of the many problems and projects that distinguishes the GPS and Vehicle Dynamics Lab from the many other university research labs. vii Finally, much gratitude is owed to Kimberly Majercik. She stood by me when stresses of life seemed overwhelming, and I thank her for being patient and understanding throughout the graduate education process. Most of all, I thank her for accepting me as a companion throughout life, and I look forward to the many experiences we will share together in the future. viii Style manual or journal used: ASME Journal of Dynamics Systems, Measurement, and Control. Computer software used: Microsoft Word 2003. ix TABLE OF CONTENTS List of Figures................................................................................................................... xii List of Tables .................................................................................................................... xv Nomenclature................................................................................................................... xvi 1. Introduction................................................................................................................. 1 1.1. Objectives and Contributions.............................................................................. 2 1.2. Outline and Prior Art .......................................................................................... 3 2. DARPA Grand Challenge........................................................................................... 6 2.1. Sensor Suite ........................................................................................................ 9 2.2. Navigation Algorithm ....................................................................................... 12 2.3. Performance ...................................................................................................... 21 2.3.1. NQE .......................................................................................................... 26 2.3.2. Grand Challenge ....................................................................................... 29 2.4. Summary........................................................................................................... 30 3. Vehicle Simulation.................................................................................................... 31 3.1. Vehicle Model................................................................................................... 31 3.1.1. Yaw Equations of Motion Derivation....................................................... 32 3.1.2. Roll and Pitch Equation of Motion Derivation......................................... 36 3.2. Tire Model ........................................................................................................ 37 3.3. Sensor Models................................................................................................... 41 x 3.4. Model Validation .............................................................................................. 43 3.5. Summary........................................................................................................... 46 4. Analysis of Vehicle Dynamics on Navigation.......................................................... 47 4.1. Generation of Vehicle Slip................................................................................ 50 4.2. Navigation Corruption by Sideslip ................................................................... 54 4.2.1. Typical Navigation Model ........................................................................ 56 4.2.2. Modified Navigation Model ..................................................................... 57 4.2.3. Expanded Navigation Model .................................................................... 59 4.2.4. Simulation Results of Sideslip Corruption ............................................... 61 4.2.5. Experimental Results of Sideslip Corruption ........................................... 68 4.3. Navigation Corruption by Longitudinal Slip .................................................... 71 4.3.1. Navigation Model with Wheel Speed....................................................... 74 4.3.2. Navigation Model with Wheel Speed and Radar...................................... 76 4.3.3. Experimental Results of Longitudinal Slip Corruption ............................ 77 4.4. Summary........................................................................................................... 83 5. INS Aiding With A Lidar ......................................................................................... 85 5.1. Test Apparatus .................................................................................................. 86 5.1.1. Experimental Setup................................................................................... 86 5.1.2. IMU........................................................................................................... 86 5.1.3. Lidar.......................................................................................................... 87 5.2. Navigation Models............................................................................................ 89 5.3. Experimental Results ........................................................................................ 95 5.3.1. Scenario 1 ? Five State Estimator............................................................. 95 xi 5.3.2. Scenario 2 ? Six State Estimator............................................................. 100 5.4. Summary......................................................................................................... 102 6. Conclusion .............................................................................................................. 103 6.1. Summary......................................................................................................... 103 6.2. Future Work.................................................................................................... 106 6.2.1. Autonomous Navigation System Development...................................... 106 6.2.2. Navigation Corruption by Vehicle Dynamics......................................... 106 6.2.3. Lidar/INS Navigation.............................................................................. 107 References....................................................................................................................... 108 Appendices...................................................................................................................... 114 Appendix A: DARPA Grand Challenge Vehicle........................................................... 115 Appendix B: Test Vehicle.............................................................................................. 120 Appendix C: Vehicle Simulation Parameters ................................................................ 124 xii LIST OF FIGURES Figure 2.1: The 2004 Grand Challenge entrants did not perform as hoped....................... 7 Figure 2.2: Team Terramax crosses the finish line............................................................ 8 Figure 2.3: A Rockwell Collins GIC-100 and Navcom Starfire DGPS receiver............. 10 Figure 2.4: Measurements from the magnetometers compared to GPS. ......................... 12 Figure 2.5: EKF position estimate during a simulated outage......................................... 22 Figure 2.6: Position estimate error during the simulated outage. .................................... 23 Figure 2.7: Heading estimate with and without magnetometer measurements. .............. 24 Figure 2.8: Discrepancy between GPS course and estimated heading. ........................... 26 Figure 2.9: Dead reckoning after a 200 foot tunnel at the NQE...................................... 27 Figure 2.10: The vehicle approaching and traveling through the tunnel. ........................ 29 Figure 2.11: The Grand Challenge course. ...................................................................... 30 Figure 3.1: Schematic used to develop the nonlinear yaw model.................................... 32 Figure 3.2: Schematic of longitudinal road grade............................................................ 33 Figure 3.3: Lateral road grade depiction.......................................................................... 33 Figure 3.4: Dugoff tire curve with varying normal loads. ............................................... 39 Figure 3.5: Front tire curve. .............................................................................................. 40 Figure 3.6: Rear tire curve. ............................................................................................... 40 Figure 3.7: Comparison of simulated and measured roll angle. ...................................... 44 Figure 3.8: Simulated yaw rate compared with measured yaw rate. ............................... 45 xiii Figure 3.9: Measured and simulated sideslip................................................................... 45 Figure 4.1: Lateral tire curve saturating on different surfaces......................................... 51 Figure 4.2: Bicycle schematic showing slip angles. ........................................................ 53 Figure 4.3: Heading estimates of the different navigation models from simulated data. 62 Figure 4.4: Yaw gyro bias estimates from simulated data............................................... 63 Figure 4.5: Jump in velocity estimate due to neglected terms......................................... 64 Figure 4.6: Corrupted longitudinal accelerometer bias estimate. .................................... 64 Figure 4.7: Roll estimates from the different navigation models. ................................... 65 Figure 4.8: Position error during an outage. .................................................................... 67 Figure 4.9: Estimated sideslip during an outage.............................................................. 68 Figure 4.10: Heading estimates of the navigation models with experimental data. ........ 69 Figure 4.11: Position error during an artificial GPS outage. ........................................... 70 Figure 4.12: Heading estimates during a GPS outage. .................................................... 71 Figure 4.13: Tire coordinate frame. ................................................................................. 73 Figure 4.14: Velocity estimate corrupted by wheel slip. ................................................. 78 Figure 4.15: Estimate with wheel speed bias removed.................................................... 79 Figure 4.16: Estimated wheel speed bias before and during a GPS outage..................... 80 Figure 4.17: Velocity estimate using wheel speed and bias estimate during an outage. . 81 Figure 4.18: Estimated vehicle velocity with Doppler radar. .......................................... 82 Figure 4.19: Doppler radar bias on different terrain........................................................ 83 Figure 5.1: Corner detection with a Lidar........................................................................ 88 Figure 5.2: Calculating lateral offset. .............................................................................. 89 Figure 5.3: Lateral distance measurement. ...................................................................... 94 xiv Figure 5.4: Velocity estimate using a Lidar..................................................................... 96 Figure 5.5: Heading estimate using a Lidar..................................................................... 96 Figure 5.6: Lateral error estimated using an IMU and Lidar........................................... 97 Figure 5.7: Estimated longitudinal accelerometer bias.................................................... 98 Figure 5.8 Estimate of the yaw rate gyro bias. ................................................................ 98 Figure 5.9: Lateral error estimate assuming no lateral velocity and ? ylat =0.01................ 99 Figure 5.10: Lateral error estimate assuming no lateral velocity and ? ylat =0.1.............. 100 Figure 5.11: Velocity estimated with second navigation model.................................... 101 Figure 5.12: Scenario 2 heading estimate...................................................................... 101 Figure 5.13: Lateral error estimate using lateral acceleration input. ............................. 102 Figure A.1: RASCAL at the starting gate during the 2005 DGC. ................................. 115 Figure A.2: Steering motor; throttle and brake servos................................................... 116 Figure A.3 Flow of information on RASCAL............................................................... 117 Figure A.4: Obstacle detection sensors on RASCAL.................................................... 118 Figure B.1: Infiniti G35 with data logging console. ...................................................... 120 Figure B.2: The Crossbow IMU; Starfire and Beeline. ................................................. 121 Figure B.3: The G35 data logging computer and power distribution center. ................ 122 xv LIST OF TABLES Table 2.1: Process Noise Statistics .................................................................................. 18 Table 2.2: Measurement Noise Statistics......................................................................... 21 Table 3.1: Sensor Characteristics Used in the Sensor Model .......................................... 43 Table 4.1: Process Noise Variances for Sideslip Corruption Investigation..................... 61 Table 4.2: Measurement Noise Variances for Sideslip Corruption Investigation ........... 61 Table 5.1: Process Variance Values for Lidar/INS Aiding.............................................. 92 Table 5.2: Measurement Variance Values for Lidar/INS Aiding.................................... 92 NOMENCLATURE Chapter 2 A Jacobian of nonlinear vector field B w Process disturbance input matrix C Measurement matrix E ? Estimated east position L Kalman filter gain matrix N ? Estimated north position P Covariance estimate Q Continuous process noise matrix R Discrete measurement noise matrix V ? Estimated velocity 1 ? ?M b Estimated TCM2 magnetometer pitch bias 1 ? ?M b Estimated TCM2 magnetometer roll bias 1 ? ?M b Estimated TCM2 magnetometer yaw bias 2 ? ?M b Estimated Microstrain magnetometer pitch bias 2 ? ?M b Estimated Microstrain magnetometer roll bias 2 ? ?M b Estimated Microstrain magnetometer yaw bias xvi r b ? Estimated yaw rate gyro bias ? b ? Estimated pitch rate gyro bias ? b ? Estimated roll gyro bias r Measured IMU yaw rate x? Estimated state vector y Measurement vector ? ? Estimated vehicle pitch g ? ? Estimated longitudinal road grade IMU ? & Measured IMU pitch rate ? Standard deviation ? ? Estimated vehicle roll M ? Roll plus lateral accelerometer measurement IMU ? & Measured IMU roll rate ?? Estimated vehicle heading Chapter 3 CG Center of gravity C ? Tire cornering stiffness xvii xviii C s Tire longitudinal stiffness F x Longitudinal drive force F xBL Longitudinal back left tire force F xBR Longitudinal back right tire force F xFL Longitudinal front left tire force F xFR Longitudinal front right tire force F y Lateral cornering force F yBL Lateral back left tire force F yBR Lateral back right tire force F yFL Lateral front left tire force F yFR Lateral front right tire force F z Normal force F zBL Normal back left tire force F zBR Normal back right tire force F zFL Normal front left tire force F zFR Normal front right tire force I x Roll inertia of the vehicle I y Pitch inertia of the vehicle I z Yaw inertia of the vehicle K p Pitch stiffness xix K r Roll stiffness L Vehicle wheelbase L D Dugoff coefficient V Velocity V BL Velocity of back left tire V BR Velocity of back right tire V FL Velocity of front left tire V FR Velocity of front right tire V x Longitudinal velocity V y Lateral velocity a Distance from center of gravity to front axle b Distance from center of gravity to rear axle b ? Walking bias c ? Constant turn on bias g Gravity k f Front spring rate k r Rear spring rate m Vehicle mass r Yaw rate s Longitudinal tire slip xx s f Vehicle front suspension width s r Vehicle rear suspension width t f Vehicle front track width t r Vehicle rear track width w ? Gaussian sensor noise ? Lateral tire slip ? BL Back left tire slip angle ? BR Back right tire slip angle ? FL Front left tire slip angle ? FR Front right tire slip angle ? Sideslip ? Steer angle ? Roll angle ? g Lateral road grade ? Clean sensor measurement ? m Dirty sensor measurement ? ? Bias noise standard deviation ? ? Bias time constant ? Gaussian noise on bias ? Pitch angle xxi ? g Longitudinal road grade Chapter 4 R CG/T Position vector from tire to center of gravity V CG Velocity at the center of gravity V T Velocity at the tire e Position Position estimate error Chapter 5 V m Velocity measurement from Lidar y lat Lateral error in global frame y V Vehicle lateral position y d Lateral distance from vehicle to corridor ? m Heading measurement from Lidar 1 1. INTRODUCTION The popularity of autonomous ground vehicles, or AGVs, is quickly increasing with the advent of better and more reliable technology. There is a large niche for such a vehicle, ranging from agricultural machines, to civilian and military transportation, to unmanned combat machines, to search and rescue vehicles, and so on. A critical component for these AGVs is a navigation system to provide vehicle and position information. A control system must know an AGV?s precise location, either global or local, as well as speed and direction of travel, in order to properly direct the vehicle and accomplish the desired objective. The control system can also utilize some vehicle state estimates from a navigation system to maintain vehicle stability as the vehicle encounters unfavorable terrain or while operating aggressively. Obstacle detection algorithms also need the navigation information to correctly map perceived obstacles in a vehicle or global coordinate frame so the vehicle will successfully and efficiently avoid anything in the path of travel. In parallel with the autonomous vehicle boom, civilian vehicle control systems are expanding their capability to compensate for driver inadequacies and ensure passenger safety. A need currently exists for more vehicle information to improve control robustness and reliability. Currently, there are few methods to accurately and/or cheaply measure lateral vehicle motion (Daily, 2004). In the near future, such control systems will rely upon improved navigation technologies to provide information about the 2 vehicle, its movement, and its position. For instance, a lane keeping control system could use the position information to control the vehicle and remain in the lane. Eventually, passenger vehicles might become autonomous on selected roads or highways, which would lead to reduced accident rates and driver fatalities. The foundation for passenger vehicle automation is a solid navigation system. 1.1. OBJECTIVES AND CONTRIBUTIONS To meet the stringent requirements of AGV and future passenger vehicle control systems, current technologies will need to be refined and hardened, and new methods need to be developed to adequately and safely navigate through many environments and over varying terrains. The subsequent chapters of this thesis detail commonly used ground vehicle navigation systems, along with frequently used sensors which measure basic vehicle motion. The inadequacies of the navigation systems and sensors are investigated, and methods to improve navigational accuracy by expanding current systems are proposed. An improvement in the accuracy of vehicle position and other vehicle information is seen by expanding the navigation model. In addition, a new method to provide a lateral error using a laser scanner is offered. This lateral error measurement could be used in a vehicle controller to enhance path following capabilities. For instance, waypoint following controllers can potentially become unstable if the distance between waypoints is too large and the angle of approach to the desired waypoint is too large. Oscillations occur if this scenario is realized, and 3 eventually the system can become destabilized. A lateral error measurement can be used by the controller to effectively damp out any oscillations caused by a large angle of approach to the next waypoint. By altering the navigation model, a lateral velocity measurement can be estimated, which could be used in the expanded navigation model mentioned above. Also, a laser scanner can be used to overcome the shortcomings of navigation systems in urban environments, as the stable bias of the sensor provides a suitable alternative to GPS in some situations. However, the laser scanner could still be augmented with GPS if desired. 1.2. OUTLINE AND PRIOR ART Chapter 2 briefly describes participation in the DARPA hosted Grand Challenge. To compete in this event, a completely autonomous vehicle was built capable of traversing rough terrain in a desert environment (Behringer, 2004, 2005; Ozguner, 2004). A navigation system was constructed to provide vehicle and navigation information to various system components. While analyzing the performance of the navigation system, it was noticed the dynamics of the vehicle introduced errors in the estimated vehicle state information. Scheding (1999) forewarned that neglecting some vehicle dynamics may inject errors into the navigation system, but no analysis of the errors was carried out. Fortunately the system as a whole was robust enough to compensate for the small inaccuracies. However, aggressive vehicle operation could have induced larger errors that would quickly propagate through the autonomous system creating undesired 4 behavior. These phenomena were investigated and solutions to improve their robustness are suggested in the following chapters. To thoroughly investigate navigation errors caused by vehicle dynamics, a vehicle simulation was created to replicate the effects in a controlled setting. Chapter 3 provides the derivation of the vehicle model and sensor models used in the simulation. The model is validated by comparing it to experimental data. Chapter 4 describes typical navigation systems and proposes an expanded navigation architecture which captures some of the vehicle dynamic effects that cause navigation error. The different systems are first analyzed with the simulation to better explain how their outputs are corrupted by the vehicle?s dynamics. The systems are then analyzed with experimental data to prove their existence in real world scenarios. Julier (2003) proposed a method which utilized a vehicle model to estimate the error inducing dynamics in a navigation algorithm. This thesis, however, utilizes a direct measurement of the dynamics in a kinematic navigation model. Outputs of common navigation sensors can also provide inaccurate measurements under certain conditions. Their effect on the navigation system is assessed in this chapter as well. The Grand Challenge also provided knowledge about a variety of sensors for obstacle detection. A method is proposed in Chapter 5 which uses a common obstacle detection sensor in a navigation system. Specifically, a laser scanner is used, but the concept is open to other types of environmental sensors, such as cameras, radar, or sonar. Laser 5 scanners currently serve several purposes on ground vehicles, and their bias is relatively stable (Ye, 2002). Simultaneous localization and mapping (SLAM) is a technique combining a laser scanner with GPS and/or INS to map obstacles as the vehicle is traveling (Guivant, 2000; Hirokawa, 2004; Talaya, 2004). This work differs from SLAM as it draws information from the environment to estimate a local lateral error. This method could improve navigational robustness in environments that typically stress current navigation systems relying on GPS and/or INS measurements, but could also be coupled with a variety of sensors to enhance the system. Hirokawa (2004) demonstrated a navigation system utilizing a laser scanner and carrier phase GPS measurements. 6 2. DARPA GRAND CHALLENGE The Defense Advanced Research Projects Agency (DARPA) hosted an event called the Grand Challenge in March of 2004 to spur technological growth in the area of unmanned ground vehicles. The intent of the Grand Challenge was to have teams build an autonomous ground vehicle (AGV) to compete in a timed race crossing different terrain types and encountering obstacles for a one million dollar prize. The inaugural event did not have the desired outcome. The furthest distance a competitor achieved was 7 out of 142 miles. Most vehicles did not make it out of the starting gate. Auburn University teamed with a limited liability corporation named SciAutonics to enter a vehicle dubbed RASCAL (Robust, Autonomous, Sensor Controlled All terrain Land vehicle). More information on this autonomous system can be found in Appendix A. Figure 2.1: The 2004 Grand Challenge entrants did not perform as hoped. DARPA again hosted the Grand Challenge in October of 2005 in hopes experience gained in the first event would propel competitors to produce more capable vehicles. Knowledge from the first event proved beneficial to teams as five completed the 140 mile Grand Challenge course in 2005. Auburn University again partnered with SciAutonics to enter the small AGV as a competitor. The additional time helped improve the vehicle controller over a broad range of speeds and develop a more complex navigation system that provided information to the different vehicle subsystems. Auburn University also partnered with Team Terramax and assisted by providing the initial vehicle controller as well as vehicle software models. Figure 2.2 displays Team Terramax crossing the finish line demonstrating the 2005 Grand Challenge was a success (Ozguner, 2004). 7 Figure 2.2: Team Terramax crosses the finish line. Vehicle navigation was a critical component of the vehicles in the DARPA Grand Challenge. It provides other systems with state information about the vehicle, such as speed, orientation, and position. Therefore, the navigation solution had to be accurate, as well as robust to disturbances that would affect its accuracy. The Grand Challenge courses offered challenges to navigation systems by limiting GPS availability and crossing terrain capable of causing vehicle slip. Errors in the navigation solution can quickly propagate throughout all vehicle subsystems causing the vehicle to drive to incorrect points and to incorrectly position located obstacles. 8 9 2.1. SENSOR SUITE The Grand Challenge entry vehicle contained a variety of sensors to determine states critical to the vehicle controller such as position, heading, and speed. A strategy of redundancy was employed to provide measurements from some sensors when others were not available or in the event of the failure of a particular sensor. The cornerstone of vehicle localization was a single antenna Navcom Starfire DGPS receiver. It generated unbiased measurements of position (north and east), velocity (north, east, and up), and course at 5 Hz. With the corrections broadcast by Navcom, this GPS receiver is capable of producing position measurements accurate to less than 10 cm. However, the output rate of the receiver was too low to adequately control the vehicle. An inertial measurement unit (IMU) was used to obtain high update rate measurements. A Rockwell Collins GIC-100 tactical grade 6 degree of freedom IMU measured linear accelerations and angular rates at 50 Hz. These measurements, however, were inherently corrupted with biases and noise. Dead reckoning with the unit provides acceptable results for a short period of time if the initial biases are eliminated, but the biases are not constant and therefore must be continually estimated and removed from the measurement to provide a reliable navigation solution. Figure 2.3: A Rockwell Collins GIC-100 (left) and Navcom Starfire DGPS receiver (right). The ATV?s onboard speedometer was used as an additional speed sensor. The output rate of this sensor was dependent upon vehicle speed, so compensation was needed to provide a more consistent measurement to the controller. The sensor waited until a certain number of revolutions of the drive shaft occurred before sending the measurement to the computer. It did not report speeds less than 1.5 m/s either. Also, the measurement was corrupted by wheel slip, which appeared as a sudden, large change in the bias if the bias was not modeled as a function of time. To calibrate the sensor, a scale factor was calculated to match the speedometer output to GPS velocity. This scale factor was not constant across all ranges of speed and would corrupt the speed estimate during a GPS outage. Magnetometers are often used in aerial applications to provide orientation information. They sense the earth?s magnetic field, thus all measurements are referenced from magnetic north and not true north. This difference can easily be calibrated and remains fairly constant if the sensor remains in a region near its calibration point. The AGV utilized two magnetometers to measure the vehicle?s heading, roll, and pitch 10 11 angles. A TCM2 magnetometer provided 16 Hz measurements that contained high noise but a slow bias drift rate. A Microstrain 3DM-GX1 IMU and magnetometer provided 50 Hz measurements that had very little noise, but the bias drifted quickly (the IMU was not used). Figure 2.4 displays vehicle yaw measurements from the two magnetometers and GPS. The quickly changing bias in the Microstrain magnetometer is immediately evident. The TCM2 magnetometer bias was fairly constant during this run but the noise statistics varied. It was discovered that the magnetometers could help initialize the navigation algorithm, but once the vehicle started moving they were of little use because the magnitude and drift rate of their biases and changing noise statistics was greater than that of the other sensors. 70 80 90 100 110 120 20 40 60 80 100 120 140 160 Time (s) Ya w ( d e g ) TCM2 Microstrain GPS Figure 2.4: Measurements from the magnetometers compared to GPS. 2.2. NAVIGATION ALGORITHM The localization algorithm used was an extended Kalman?Bucy filter (EKF), outlined in detail by Stengal (1994). This method assumes the system can be represented in the following form: () ()vuxhy wuxfx += += , ,& 2.1 where the Jacobian of the vector field is 1? = ? ? = ? ? k x A x x x f & 2.2 12 and the measurement matrix is 1? = ? ? k x C x h 2.3 The process noise covariance matrix denoted by Equation 2.4. [ ] )(?QwwE T = 2.4 It is important to realize that Q must be continuous when used with this specific algorithm. The discrete conversion occurs within the Kalman filter, using the process noise input matrix, B w . 1? = ? ? k x w B w x& 2.5 The matrix B w was set to identity to add process noise to all states. The measurement noise covariance matrix is defined in a similar manner as Q. [ ] )( k T tRvvE = 2.6 The covariance of the estimate error is defined in Equation 2.7. ()()[ ] )(?? k T tPxxxxE =?? 2.7 Note this equation requires information about the actual state, x, which is not available. However, the Kalman filter is able to estimate the covariance of the estimate error using the A, C, Q, and R matrices. The Kalman?Bucy filter algorithm handled the system nonlinearities by continuously propagating the system model and discretely propagating the measurement update, as shown in the following equations. 13 State Estimate Continuous Propagation ? ? ? ? ? ? ? ? ? + + ? = ? k t k t duxf k tx k tx 1 ),(),(?) 1 (?)(? ???? 2.8 Covariance Estimate Continuous Propagation ??????? d T w BQ w B k t k t T APPA k tP k tP ? ? ? + ? ? ? ? ? ++ + ? = ? )()( 1 )()()()() 1 ()( 2.9 Kalman Filter Gain Calculation 1 )()()()()()()( ? ? ? ? ? ? ? + ?? = k tR k tC k tP k tC k tC k tP k tL TT 2.10 State Estimate Discrete Update ? ? ? ? ? ? ? ?+ ? = + )(?)()()()(?)(? k tx k tC k ty k tL k tx k tx 2.11 Covariance Estimate Discrete Update [ ] )()()()( ? ?= + k tP k tC k tLI k tP 2.12 Kalman filtering is a proven method for blending measurements to eliminate various sensor deficiencies while utilizing the strengths of each sensor by statistically weighting each measurement. The EKF combined the bias free, low update GPS measurements with the other measurements to produce a bias free, high update navigation solution. An EKF is as accurate and less computationally intensive as some higher order filters if the sample rate is high enough (St. Pierre, 2004). This efficiency was an advantage with the algorithm because the state vector already imposed a moderate computational burden by containing 16 states. GPS and inertial measurements were loosely coupled, meaning the 14 15 inertial errors were corrected with a computed GPS solution. A tightly coupled system was considered, where the raw GPS pseudo-range measurements amend the IMU errors, but due to time constraints and the overall satisfaction with the loosely coupled system, it was not constructed. The geometric relationships between the sensors on board the AGV were easily and accurately measured. Using this information, kinematic relationships between the sensors were derived. These relationships, otherwise known as a kinematic model, made up the vector fields listed in Equation 2.1. A vehicle model could have been used in place of the kinematic model, but knowledge of vehicle parameters would have been required. For the AGV, the parameters were either unknown, or had the potential to vary, so the kinematic model improved the robustness of the navigation solution by relying only on the kinematic relationships between the sensors. The kinematic model evolved with the vehicle as new sensors were added. The first model was a simple, four state linear model estimating the vehicle?s velocity, course, longitudinal accelerometer bias, and gyro bias. The second model added states of north position and east position, which are described in the Kalman filter with a first order, nonlinear differential equation. The nonlinearities required the use of the EKF mentioned above. The third and fourth models were intended for use during the National Qualification Event (NQE) and the Grand Challenge. Those models used kinematic relationship between lateral acceleration, velocity, and yaw rate, to provide an effective 16 measurement of roll, and the GPS up and forward velocity to generate a road grade measurement. Using this effective measurement, states of roll plus lateral accelerometer bias, pitch, and road grade became observable (Bevly, 2004). The fourth model utilized roll, pitch, and yaw measurements from the two magnetometers to add redundancy to the estimator. The need of the third model existed because magnetometer problems were experienced during the 2004 Grand Challenge due to their sensitivity to nearby metallic objects. However, only the fourth model is presented in this work since it was actually used during the events. A navigation algorithm with a sixteen state kinematic model was constructed for use in the NQE and DARPA Grand Challenge event. This model estimated velocity, longitudinal accelerometer bias, course, yaw rate gyro bias, north and east position, roll, pitch, and road grade, as well as IMU and magnetometer biases when GPS was available. During a GPS outage or iterations where GPS was not available, the navigation algorithm dead reckoned using the IMU, wheel speed sensor, and magnetometers with their respective biases removed. The vehicle controller used the estimated heading and velocity states to guide the vehicle along the desired path. The orientation states of roll, pitch, road grade, and heading, were used to transfer the data recorded from the obstacle detection sensors from the vehicle frame to a global frame. The nonlinear differential equations for this model are shown in Equation 2.13. )?sin( ?? )?cos( ?? 0 ? ? ? ??? ? ? ? ?? VE VN b br ggaV r r gx = = = ?= ??= & & & & & 0 ? 0 ? ?? 0 ? ?? = = ?= = ?= g IMU IMU b b b b ? ?? ?? ? ? ? ? & & & & & & & 0 ? 0 ? 0 ? 0 ? 0 ? 0 ? 2 2 2 1 1 1 = = = = = = ? ? ? ? ? ? M M M M M M b b b b b b & & & & & & 2.13 These nonlinear equations were continuously propagated in the EKF time update of the state estimate. However, the time propagation of the covariance estimate required a linear system matrix. The linear system matrix used the Jacobian of the nonlinear model, which was calculated with Equation 2.2. The matrix shown below is the linear system matrix used in Equation 2.9. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? = 0000000000000000 0000000000000000 0000000000000000 0000000000000000 0000000000000000 0000000000000000 0000000000000000 0000000000000000 0000000100000000 0000000000000000 0000000001000000 00000000000000)?cos( ? )?sin( 00000000000000)?sin( ? )?cos( 0000000000000000 0000000000000100 00000000000000 ?? ?? V V gg A 2.14 17 Biases were modeled as random walks in the process noise matrix. These were used as tuning parameters for the EKF because they directly influenced the amount of filtering on the estimated states. The other entries in the matrix captured the system noise, which was determined by gathering static sensor measurements. Assuming the sensor noises are uncorrelated, the process noise covariance matrix is a diagonal matrix with the covariances in Table 2.1. The time update in the EKF assumes this matrix to be continuous. Therefore the measured, discretized values are multiplied by the sample rate (dt) in order to approximate the continuous covariance values. Table 2.1: Process Noise Statistics 22 0.65 az dt? = 22 0.038129 r dt? = 21 10 br ? 0? = 22 0.17851 dt ? ? = & 21 10 b? ? ? = 0 22 0.088147 dt ? ? = & 28 10 b? ? ? = 22 0.000001 N dt? = 22 0.000001 E dt? = 27 10 g b? ? ? = 24 1 10 bM? ? ? = 26 1 10 bM? ? ? = 28 1 10 bM? ? ? = 2 2 0.01 bM? ? = 2 2 0.01 bM? ? = 2 2 0.1 bM? ? = The discrete measurement update in the EKF utilized statistically weighted measurements from the sensors listed in Section 2.1 to overcome integration errors. Two calculations were included in addition to the raw sensor measurements. GPS velocities were used to solve for road grade using Equation 2.15. 18 GPSx GPSup g V V =? 2.15 Linear equations of vehicle roll as a function of lateral acceleration were derived using knowledge of the vehicle?s dynamics and a basic sensor model. A basic sensor model can be written as a scale factor on the actual state, plus bias (constant and/or moving), plus noise. wbSFxy ++= 2.16 Applying this equation to a lateral accelerometer measurement leads to the following equation. ( ) wbVrgVSFa ayMyy ++++= )sin(? & 2.17 Under the small angle assumption, the V y term goes to zero because vehicle sideslip can be ignored, and the roll component can be simplified. The yaw rate measurement, r, will have its own inherent scale factor and bias which need to be removed. Manipulating Equation 2.17, applying the small angle approximation, and removing the scale factors leads to a measurement of vehicle roll plus lateral accelerometer bias. () ayryM gbbrVa g ???= )( 1 ? 2.18 The measurements for the 16 state EKF are listed in Equation 2.19. [ ] T MAGMAGgMAGMAG MMAGMAGGPSGPSGPSWSGPS ENVVy 2121 21 ... ????? ????= 2.19 19 The measurement matrix, C, was adjusted accordingly depending upon the availability of the different measurements. The vehicle computing system was run at 50 Hz, but some sensors had slower output rates. For instance, GPS measurements were output at only 5 Hz. When a measurement was unavailable, its corresponding value was set to zero. States dependent upon an unavailable measurement were unobservable, and the algorithm integrated the kinematic model to provide estimates. The resulting measurement matrix when all measurements are available is shown in Equation 2.20. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 0010000000000010 0000010000000010 0000001000000000 1001001000000000 0000001000000000 0000000000100000 0100000000100000 0000100000100000 0000000000010000 0000000000001000 0000000000000010 0000000000000001 0000000000000001 C 2.20 Table 2 displays the values used in the measurement covariance matrix, R. These values were experimentally obtained by calculating the standard deviation of a static data set. However, the noise statistics for GPS course and GPS road grade are a function of the GPS velocity noises (Bevly, 2004). 20 Table 2.2: Measurement Noise Statistics 22 0.05 V ? = 22 0.3 VWS ? = 2 2 V V ? ? ? ?? = ?? ?? 22 0.1 N ? = 22 0.1 E ? = 2 2 g V V ? ? ? ?? = ?? ?? 22 0.08135 calc ? ? = 22 1 0.19995 M? ? = 22 1 0.024445 M? ? = 22 1 0.036297 M? ? = 22 2 0.003 M? ? = 22 2 0.003 M? ? = 22 2 0.003 M? ? = 2.3. PERFORMANCE The algorithm?s performance was evaluated based on the amount of error during a simulated GPS outage. Two reasons for this evaluation method are as follows: first, when enough satellites are in view and the receiver is outputting valid messages, the EKF successfully tracks the GPS measurements, and second, a real GPS outage would eliminate the truth measurement, degrading the accuracy of the error analysis. Figure 2.5 is a plot of GPS and estimated position during a test run. The circles signify the beginning and end of the simulated outage, starting before the first turn and concluding at the end of the straight section. 21 40 50 60 70 80 90 100 110 -40 -35 -30 -25 -20 -15 -10 East (m) N o rth (m ) GPS EKF Figure 2.5: EKF position estimate during a simulated outage. The error growth which occurs at the onset of the outage can be seen in the above figure. Figure 2.6 is a plot of the navigation error. Over the 25 second outage, the vehicle was traveling 3.2 m/s. The maximum error for this period of time at this speed was slightly less than a meter. 22 380 390 400 410 420 430 440 450 -0.2 0 0.2 0.4 0.6 0.8 1 E rro r (m ) Time (s) Figure 2.6: Position estimate error during the simulated outage. The IMU and speedometer were the sensors whose biases impacted the navigation solution the most. The leading error source during this run was due to the calibration error in the speedometer. The vehicle experienced slight lateral translation, or slipped, as it rounded the corner, also introducing a small navigation error. These two error sources are deduced by eliminating other possible error sources. The tactical grade IMU contained mechanical rate gyros with a bias drift rate of less than one degree per square- root-hour, so the bias error over this period of time is very small. Therefore, error due to IMU bias drift can be assumed to be negligible. As stated earlier, the magnetometers were not much use during a run and were statistically weighted out of the EKF after 23 initialization. Therefore they did not have much impact on the navigation solution once the vehicle was in motion. To prove the magnetometers were ignored once the vehicle was moving, a Kalman filter that did not use magnetometer measurements was compared to the algorithm on the AGV. Figure 2.7 shows the output of the navigation system utilizing the magnetometers compared to the system that did not use the information. When the vehicle is moving, there is virtually no difference in the estimated heading. 80 85 40 50 60 70 80 90 100 110 120 130 140 Heading H ead i n g (d eg ) GPS No Mag Mag Figure 2.7: Heading estimate with The effects of the calibration error could be wheel speed bias was estimated, the calib when the bias was modeled with high enough dynam 90 95 100 105 110 Time (s) and without magnetometer measurements. removed when GPS was available. If a ration error effects would appear in the estimate ics. However, when GPS was lost, 24 25 bias states were held constant because they were unobservable. Therefore if the vehicle changed speeds during an outage, this bias estimate would be incorrect because the calibration error would change proportionally with speed. The navigation algorithm on the AGV without the estimated bias state was compared with another Kalman filter that included this bias state. It was determined there was no benefit to including the bias state because the dead reckoning errors resulting from each estimator were similar. Another source of navigation error was attributed to vehicle slip. Vehicle slip can disrupt the navigation algorithm even in the presence of GPS. Longitudinal and lateral slip occur on moving ground vehicles. Longitudinal slip is generated by a difference in the vehicle?s velocity and the wheel?s velocity, and lateral slip is created when the vehicle translates laterally. Wheel slip can corrupt the speedometer measurement by causing a sudden jump in the estimated bias (if the modeled bias dynamics have a high enough bandwidth). Also, it reports a false speed value to the Kalman filter which can directly inject error into the speed and position estimates. Sideslip also leads to a less accurate estimate because GPS and integrated IMU measurements differ. Evidence of sideslip induced error can be seen in Figure 2.6 at 420 seconds when the vehicle enters a turn. Figure 2.8 is a plot of the estimated heading when GPS is available. At 395 and 420 seconds, a discrepancy due to sideslip can be seen between estimated heading and GPS course. 85 90 95 100 105 110 115 50 100 150 H eadi ng (de g ) GPS KF 85 90 95 100 105 110 115 -20 -10 0 10 20 E r r o r (d eg ) Time Figure 2.8: Discrepancy between GPS course and estimated heading. The error caused by vehicle slip is seen on multiple estimated states and is influenced by the level of filtering in the EKF. This phenomenon prompted further research on the influence of vehicle dynamics on navigation algorithms. Preliminary work is discussed in (Travis, 2005). Chapter 4 presents a detailed analysis showing how a navigation solution is corrupted by vehicle slip. 2.3.1. NQE The NQE was the final qualification event for the Grand Challenge. Of the 195 initial entrants, 43 teams successfully completed a pre-selection qualification and participated at the NQE. After the NQE, the best 23 teams got the opportunity to start the Grand 26 Challenge. The main goal of the NQE was to complete four different tracks, each approximately 2.5 miles long, while avoiding various obstacles. The daunting challenge to the navigation system was a 200 foot covered tunnel that blocked GPS signals. The first NQE run successfully navigated through the tunnel, but failed shortly thereafter. As the vehicle approached an obstacle, it suddenly swerved and made contact with a barrier. Upon examining the data, it was discovered the vehicle accurately dead reckoned for two minutes. Figure 2.9 shows the dead reckoning performance of the vehicle. Figure 2.9: Dead reckoning after a 200 foot tunnel at the NQE. The tunnel is outlined in white at the right of the figure. GPS waypoints are denoted by dark blue dots. The corridor the vehicle was required to remain in a given corridor, which is shown shaded in light blue. The EKF output is indicated by red dots. The EKF output remained in the center of the corridor throughout most of the outage. However, to assess the accuracy of the dead reckoned navigation solution, the areas where the vehicle avoided obstacles must be examined. As the vehicle made the left hand turn encircled by 27 28 a white oval, it successfully avoided an obstacle. Again, the vehicle avoided an obstacle after it made a right turn onto the widened corridor. Had the navigation solution been incorrect, the vehicle would not have successfully avoided these two obstacles. The cause of the failure can also be seen in Figure 2.9. When the GPS unit began receiving signals again, it reported an accuracy of 10 centimeters on the first waypoint. This information was incorrect as Starfire corrections are not instantaneous. The reported waypoint was actually 10 meters from the EKF position estimate. The GPS receiver also reported pure zeros in its calculation for velocities, which ultimately blew up the EKF. The addition of a few simple lines of code ignored these false messages at the expense of having to dead reckon for approximately four minutes after an outage. All other NQE runs were completed by the vehicle. Figure 2.10 shows the vehicle approaching and traveling through the 200 foot tunnel during the third run at NQE. Similar performance is seen as the vehicle remains in the center of the corridor. The yellow dashes represent obstacles. Figure 2.10: The vehicle approaching and traveling through the tunnel. 2.3.2. Grand Challenge The navigation system was not stressed during the actual Grand Challenge as the vehicle was shut down before it encountered any obstacles that would obstruct satellite view. Therefore, it tracked GPS velocity, course, and position for the duration of the vehicle?s operation in the event. Figure 2.11 is a plot of the DARPA Grand Challenge course in green and the estimated vehicle position as reported from the navigation system in black. Total distance traveled was approximately 16 miles before a power cable disconnected and crashed the obstacle avoidance algorithm. 29 driving direction Start / Finish elimination point Figure 2.11: The Grand Challenge course. 2.4. SUMMARY A navigation system for an autonomous AGV was developed utilizing an extended Kalman-Bucy filter. The system also provided vehicle state information to other vehicle subsystems. The vehicle was invited to participate in the DARPA Grand Challenge, where it completed 16 miles before a hardware failure occurred. While analyzing data, peculiar navigation errors were noticed when the vehicle completed turns at higher speeds. It was concluded the error sources were the vehicle dynamics, specifically vehicle slip. 30 31 3. VEHICLE SIMULATION A MATLAB? vehicle simulation was developed to replicate driving scenarios typically producing undesirable sideslip angles to study their influence on navigation systems. A derived vehicle model was used to simulate the vehicle response during input maneuvers. The simulated response was compared with experimental data to validate the simulation?s use in this study. The simulation was then used as a tool to evaluate the effect sideslip has on a Kalman filter both when GPS is and is not available. Parameters used in the simulation can be found in Appendix C. 3.1. VEHICLE MODEL A five degree of freedom vehicle model was chosen to simulate various driving scenarios. A nonlinear yaw model was chosen to replicate the dynamics when a vehicle experiences large sideslip angles. Linear roll and pitch models were developed using the small angle assumption that the roll and pitch angles would be less than 10 degrees. Inputs into the simulation were a velocity profile (V), steer angle profile (?), and time vector. The GPS velocity, filtered using a forward-backward filter to attenuate noise, and steer angle were used as inputs to simulate an experimental run. The physical dimensions of the car such as front and rear track width (t f and t r ), distance from the front and rear axle to the CG (a and b), and suspension position (s f and s r ) were measured. Front and rear spring rates (k f and k r ) were also measured. Inertias (I x , I y , and I z ) and CG height (h CG ) were approximated. Values for the model parameters, as well as details about the experimental test bed, are given in Appendix B. 3.1.1. Yaw Equations of Motion Derivation The equations of motion for the yaw model were found by summing forces and moments about the CG. A vehicle schematic about the yaw axis is shown in Figure 3.1. ? BL ? BR ? FL ? FR ab t r t f F xBL F xBR V FR V FL F xFL F xFR V BR V BL F yFL F yFR F yBR F yBL ? ? V x V V y ? r x y z Figure 3.1: Schematic used to develop the nonlinear yaw model. Summing the moments about the CG yields the following equation: [] ()() yBRyBLxBRxBL r xFRxFLyFRyFL yFRxFRyFLxFL f zCG FFbFF t FFFFa FFFF t rIM +??+ ++++ +??==? 2 )sin()sin()cos()cos( )sin()cos()sin()cos( 2 ???? ????& 3.1 Road grade in the longitudinal direction is denoted by ? g . The effects must be accounted for when deriving the equations of motion. 32 ? g Z z X x Figure 3.2: Schematic of longitudinal road grade. Summing forces in the x direction yields Equation 3.2. )sin()cos()cos( )sin()sin( gxFRxFL xBRxBLyFRyFLx mgFF FFFFxmF ??? ?? ?++ ++??==? && 3.2 Lateral road grade, ? g , has a similar effect to longitudinal road grade, as shown by Figure 3.3. Figure 3.3: Lateral road grade depiction. Equation 3.3 is found by summing forces along the y axis. )sin()sin()sin( )cos()cos( gxFRxFL yBRyBLyFRyFLy mgFF FFFFymF ??? ?? +++ +++==? && 3.3 The acceleration vector consists of a component in line with the vehicle?s direction of travel ( ), and a centripetal component ( ). The vector is translated to the vehicle frame for use in the equations of motion. T a v N a v 33 NT aaa vv v += 3.4 Equation 3.4 can be rewritten to include the variables defined in Figure 3.1. VVa v & v v ?+= ? 3.5 The velocity vector is split into x and y components. )sin( )cos( ? ? VV VV y x v v = = 3.6 Velocity in the z axis is assumed to be zero for this study. The time derivative of Equation 3.6 yields the acceleration component in the vehicle?s direction of travel. )cos()sin( )sin()cos( ??? ??? & v & v & & v & v & VVV VVV y x += ?= 3.7 The centripetal acceleration due to rotation about the z axis, or yaw rate, is translated to the vehicle frame using the following equation. ( ) ( ) yx VrVrV )sin()sin( ??? vvv +?=? 3.8 Combining the acceleration components yields the vehicle?s longitudinal acceleration. ())sin()cos( ??? rVVx +?= & v & v && 3.9 Lateral acceleration in the y direction is found in the same manner. () )cos()sin( ??? rVVy ++= & vv && 3.10 Substituting the x acceleration equation into Equations 3.2 yields: ( ) ( ) ( ) () ? ? ? ? ? ? +?= ++++?? )sin()cos( )cos()sin( ??? ?? rVVm FFFFFF xFRxFLxBRxBLyFRyFL & v & v 3.11 and substituting the y acceleration given in Equation 3.10 into Equation 3.3 yields: 34 ( ) ( ) ( ) ()[])cos()sin( )sin()cos( ??? ?? rVVm FFFFFF xFRxFLyBRyBLyFRyFL ++= +++++ & vv 3.12 The total drive force is assumed to be split equally among the rear tires as seen in Equation 3.13. xBRxBLx FFF += 3.13 The following equations are found by solving for yaw acceleration, sideslip rate, and total drive force at the rear axle using Equations 3.1, 3.11, and 3.12. Yaw Acceleration )()sin()( ... )cos()()sin()( ... )cos()( 2 )( 2 yBRyBL z xFRxFL yFRyFL z yFRyFL xFRxFL z f xBRxBL z r FF I b FF FF I a FF FF I t FF I t r +??+ ++?+ ?+?= ? ?? ?& 3.14 Sideslip Rate [ ] } r V gVFF FFFF m gxFRxFL yFRyFLyRRyRL ???+ ? ? ? +++= )sin( 1 )sin()sin()sin()( ... )cos()()( 1 ? ??? ?? & v & v & 3.15 Drive Force )sin()( ... )sin()sin()()cos(( ? ???? yFRyFL gx FF grVVmF ++ ? ? ? ? ? ? ???= & v & v 3.16 In the vehicle simulation, the yaw acceleration and sideslip rate were numerically integrated to obtain yaw rate, heading, and sideslip. Once the drive force (F x ) was calculated, it was divided up among the four wheels. A positive drive force represented acceleration, and the total force was split between the two rear wheels (F x_OR , F x_IR ). A 35 negative drive force indicated braking and was split with a 60:40 ratio between the front (F x_OF , F x_IF ) and rear axles as shown in Equation 3.17. 2 6.0 , 2 4.00 2 0 x xFLxFR x xBLxBR x xBLxBR F FF F FFx F FFx ====?< ==?? && && 3.17 3.1.2. Roll and Pitch Equation of Motion Derivation Pitch (?) and roll (?) were approximated as linear, second order systems centered at the CG. Equations 3.18 and 3.19 were numerically integrated to obtain pitch and roll. These equations are valid only at small roll and pitch angles. Pitch CGpy hxmKI && &&& =++ ??? 7.0 3.18 Roll CGrx hymKI && &&& =++ ??? 7.0 3.19 When the vehicle?s front and rear axle spring stiffness (k f , k r ) are known, the vehicle?s pitch stiffness K p is defined by Dixon (1996) as the following: rfp kbkaK 22 22 += 3.20 Similarly, the roll stiffness K r is shown in Equation 3.21, where s f and s r are the distances between the left and right springs on the front and rear. )( 2 1 22 rrffr skskK += 3.21 Knowledge of the roll and pitch angles provides a more accurate dynamic calculation of the normal loads at each corner of the vehicle. In a static state, the normal loads are simply a function of the vehicle?s weight, wheel base, and track width. However, weight is transferred to and from the corners of the vehicle when the vehicle moves. Therefore 36 the normal loading becomes a function of not only the static normal load, but also includes effects from roll, pitch, and acceleration. 3.2. TIRE MODEL A Dugoff tire model was used to simulate the nonlinear tire behavior. This specific model was chosen for its ability to approximate the tire curve with inputs of lateral and longitudinal tire stiffness (C ? and C s ) and peak friction coefficient (?). This model also has the ability to couple longitudinal and lateral tire slip. As longitudinal tire slip increases, the amount of available lateral force decreases (Olson, 2003). The Dugoff tire model is shown in the following equations. Longitudinal tire slip is denoted by s, and lateral tire slip is denoted by ?. 1 otherwise )2( ,1 if )tan(2 )1( 222 = ?=< + ? = f LLfL CC sF L DDD s z D ? ? ? 3.22 Using f, the lateral and longitudinal tire force is solved given a normal load and slip angle or percentage. Lateral tire force is given by Equation 3.23. f s C F y ? ?= 1 )tan(? ? 3.23 Longitudinal force is provided using Equation 3.24. f s sC F s x ? = 1 3.24 Tire peak force is a function of normal load. Under static conditions on a level surface, the normal load at a front and rear tire is given by Equation 3.25. 37 L mga F L mgb F zRzF 2 , 2 == 3.25 However, dynamic conditions and road grade induce weight transfer due to roll and pitch. The body orientation angles described in Section 3.1.2 were used to calculate the normal load at each wheel. Lateral weight transfer was assumed to be caused by lateral acceleration and roll angle. It was calculated at the front and rear axles. ? ? ? ? ? ? ? ? +=? ? f f CG ZF k g y t h L b mgF && ? ? ? ? ? ? ? ? +=? ? r r CG ZB k g y t h L a mgF && 3.26 Longitudinal weight transfer was simulated only with longitudinal acceleration under the assumption the pitch angles remain small. This study focused on maneuvers that excited the lateral vehicle dynamics; therefore this assumption is safe because no maneuvers creating significant pitch angles were input into the simulation. L xmh F CG ZP && =? 3.27 Distributing the weight transfer effects among the four tires yields Equation 3.28. 22 ZP ZFZFL F F L mgb F ? ??+= 22 ZP ZFZFR F F L mgb F ? ???= 22 ZP ZRZBL F F L mga F ? +?+= 22 ZP ZRZBR F F L mga F ? +??= 3.28 38 Figure 3.4 displays a lateral tire curve generated by the Dugoff model with varying normal loads. No longitudinal slip was included in this curve. -50 -40 -30 -20 -10 0 10 20 30 40 50 -5000 -4000 -3000 -2000 -1000 0 1000 2000 3000 4000 5000 Tire Slip Angle (deg) L a te ra l F o rc e (N ) Figure 3.4: Dugoff tire curve with varying normal loads. The tire cornering stiffness, C ? , was experimentally determined by operating the vehicle at high speeds around corners of the track owned by Auburn University?s National Center for Asphalt Technology (NCAT). Figure 3.5 and Figure 3.6 show the linear region of the tire curve for a front and rear tire. Front and rear tire cornering stiffness was estimated to be approximately 800 N/deg and 1300 N/deg per tire, respectively. The peak friction coefficient, ?, served as a tuning parameter for the simulation, and was selected to be 0.84 for the front and 0.81 for the rear. 39 Figure 3.5: Front tire curve. Figure 3.6: Rear tire curve. 40 The tire curves were generated utilizing a sideslip measurement from a two antenna GPS receiver. The method used to measure sideslip is given in Appendix B. Once the sideslip measurement was taken, a longitudinal and lateral velocity at the CG could be moved to the individual tires using Equation 3.29. CGTCGT RrVV / vvv ?+= 3.29 Once the lateral and longitudinal tire velocities were known, the tire slip angle (?) was calculated. ?? ? ? ? ? ? ? ? ? ? = ? x y V V 1 tan 3.30 Lateral tire force was calculated using Equations 3.1 and 3.3. r L I Vr L ma F r L I Vr L mb F z yf z yf & & ?= ? ? ? ? ? ? += )cos( 1 ? 3.31 3.3. SENSOR MODELS Expressions for clean measured accelerations (without noise or biases) were found by taking the time derivative of the velocity vector at the CG and adding the centripetal acceleration effects due to rotation about each axis. The acceleration measurement in the z axis consists solely of centripetal acceleration and gravity components since the car is restricted to operate on a level surface. Longitudinal Acceleration Measurement )sin()sin()()cos( gx grVVa ????? +++?= & v & v 3.32 Lateral Acceleration Measurement 41 )sin()cos()()sin( gy grVVa ????? ++++= & v & v 3.33 Vertical Acceleration Measurement )sin()sin()cos()sin( ggz ggVVa ???????? +?+??= & v & v 3.34 Inertial sensors were simulated by adding Gaussian noise, a constant bias, and a walking bias to the raw measurement, as shown in Equation 3.35. Sensor Model ??? ?? wbc m +++= 3.35 The variable ? m denotes actual sensor output, ? is the true acceleration or turning rate, c ? is the constant turn on bias, b ? is the walking bias modeled with a Markov process given by Equation 3.36, and w ? is Gaussian noise. Markov Process ]1,0[~ , 21 2 ?? ? ? ? ? ? ? ? ? t bb ? +?= & 3.36 Inputs to the Markov process are the sensor sampling rate (?t), Markov time constant (? ? ), bias standard deviation (? ? ), and Gaussian noise (?). Values used for the time constant and standard deviation for each sensor are listed in Table 3.1 (Gebre-Egziabher, 2004). 42 43 Table 3.1: Sensor Characteristics Used in the Sensor Model Sensor Noise (w ? ) Turn On Bias (c ? ) Bias Time Constant (? ? ) Bias Variation (? ? ) Automotive Grade Rate Gyro ~N[0,1] >0.6 m/s 2 300 sec 180 ?/hr Automotive Grade Accelerometer ~N[0,1] >0.3 deg/s 100 sec 1.2x10-3 g 3.4. MODEL VALIDATION The model was first compared to actual data to validate its use as a tool to investigate how sideslip corrupts a navigation solution. The simulated data was compared with steady state maneuvers and transient maneuvers to confirm the model?s accuracy. Experimental maneuvers were performed with the Infiniti G35, which is described in detail in Appendix B. The simulation assumed the vehicle was on level terrain because of its inability to compensate for varying road grade. Also, an assumption of no longitudinal slip was made. Because of this assumption, the vehicle was put in neutral before starting the maneuver. Figure 3.7 displays the measured and simulated roll data during two abrupt lane change maneuvers. The discrepancy seen in Figure 3.7 is due to the ?2? of road crown as the vehicle moves from one lane to the other. 2400 2600 2800 3000 3200 3400 -5 0 5 Time (s) R o ll (d e g ) Acutal Simulated Figure 3.7: Comparison of simulated and measured roll angle. Figure 3.8 shows the simulated yaw rate and the actual vehicle yaw rate, while Figure 3.9 is the measured sideslip generated during this maneuver with the simulated sideslip. The small disagreements in these states are primarily due to the assumption of zero road grade and zero longitudinal slip. The experimental run traversed from the far right lane to right side of the left lane in this maneuver. Therefore the vehicle experienced approximately 2? to 0? of lateral road grade. The ?2? of crown on the road impacts sideslip significantly. A similar maneuver starting in the far left lane showed similar error for negative sideslip angles, but the positive sideslip angles were matched correctly 44 5300 5400 5500 5600 5700 -30 -20 -10 0 10 20 30 Time (s) Ya w Ra t e ( d e g / s ) Actual Simulated Figure 3.8: Simulated yaw rate compared with measured yaw rate. 5300 5350 5400 5450 5500 5550 5600 5650 -6 -4 -2 0 2 4 Time (s) Si d e s l i p ( d e g ) Actual Simulated Figure 3.9: Measured and simulated sideslip. 45 46 3.5. SUMMARY A MATLAB? simulation was written which included a nonlinear vehicle model to simulate the dynamics of the vehicle during various maneuvers. The model included vehicle?s yaw, roll, and pitch dynamics, lateral and longitudinal effects, and a nonlinear Dugoff tire model. There is good correlation between the simulated and actual measurements, therefore it can be concluded the model sufficiently captures the vehicle?s dynamics and can be used to examine the effect of sideslip on a navigation solution. 47 4. ANALYSIS OF VEHICLE DYNAMICS ON NAVIGATION The demand for accurate and reliable navigation for ground vehicles is proliferating as passenger vehicle control systems become more complex and as the market for autonomous capability continues to grow. In addition to vehicle state information, a reliance on a navigation solution will be required as vehicle stability control systems increase their intelligence beyond the current versions of Electronic Stability Control (ESC), Vehicle Dynamic Control (VDC) and so forth. Future intelligent and safety vehicle systems may use this information to provide better lane keeping capability, higher accuracy vehicle tracking, or enhanced driver assistance systems. Also, control of AGV?s requires precise navigation information. The need for reliable navigation information will increase as passenger vehicles continue to acquire AGV-like capabilities. An increase in the precision of the navigation solution can directly lead to better and more robust vehicle controllers. Vehicle dynamicists often use models to provide detailed vehicle state information to the driver or a control system. These models are often intricate, requiring parameters that are hard or expensive to measure, and can be excessively complicated for navigational purposes (Venhovens, 1999). Navigation specialists sometimes use a simplified vehicle model based on kinematic relationships. However, these models sometimes neglect key states such as lateral velocity present in the actual dynamics of the ground vehicle operating at normal speeds (Dissanayake, 2001; Godha, 2005). A tradeoff exists to 48 account for the critical states sometimes neglected by navigation models while keeping the model complexity to a minimum. This tradeoff is fundamental to the next generation of automobile control systems and AGV navigation (He, 2002; Daily, 2004). Vehicle model parameters are often hard to obtain or change frequently. For instance, tire parameters are often unknown, and the owner might change values for the mass, inertias, and CG location by loading the vehicle. Therefore, a relatively simple kinematic model capturing similar states as a vehicle model will be more robust to those model changes common to the everyday passenger vehicle. The focus of this chapter is to show how vehicle slip induces errors in a navigation solution unless accounted for both when GPS is available and when dead reckoning through a GPS outage. Many scenarios exist where non-negligible amounts of slip are generated. A vehicle traveling on low friction surfaces will quickly break the assumption of no slip. Active control systems that steer the vehicle away from an impending accident will generate large amounts of vehicle slip in order to maximize the force at the ground to quickly alter the vehicle?s path and minimize the chance of a collision. Similarly, an AGV suddenly maneuvering to avoid an obstacle can generate noticeable vehicle slip. The vehicle slip generated in both of these scenarios can produce navigation errors. A vehicle equipped with a lane keeping system traveling on an icy road might experience difficulties because the navigation solution and map database do not agree due to these navigation errors. The main contribution of this section is to show lateral vehicle slip can corrupt navigation solutions from a Kalman filter by introducing discrepancies between measured states and integrated inputs, and therefore must be taken into account for systems 49 requiring precise vehicle navigation information. Previous work has explained the importance of lateral vehicle slip in navigation through the use of vehicle models (Scheding, 1999; Julier, 2003); however the work in this thesis utilizes a kinematic navigation model to explain the consequences of neglecting slip (Travis, 2005). The contribution of this chapter requires a measurement of vehicle heading, which was obtained using a two antenna GPS receiver. Slip is calculated from raw measurements rather than estimated with a vehicle model. While a multiple antenna GPS system on a passenger vehicle might seem like a distant concept, many vehicles today already come equipped with a single antenna system. The cost of adding a second antenna would arise at the chipset level of the GPS board, which could be an inexpensive addition (Alban, 2004). Many commercial systems which provide improved positioning accuracy through differential corrections, known as DGPS, are currently available such as WAAS (Wide Area Augmentation System), Coast Guard corrections, Starfire, Omnistar, etc. In addition, systems developed by the Federal Highway Administration (FHWA) such as NDGPS and HA-NDGPS seek to provide high accuracy navigation for transportation purposes (Cook, 2000; Arnold, 2001; FHWA, 2005). To investigate the affect of vehicle slip on navigation solutions, the vehicle simulation outlined in Chapter 3 was developed to demonstrate the corruption by slip in a controlled environment. Data was also collected on a test vehicle to for further validation of how the navigation accuracy can be degraded under scenarios where slip is produced. Three navigation models were utilized to present the solution accuracy when slip was and was not accounted for. Simulated and experimental data were analyzed when navigating with and without GPS. 50 4.1. GENERATION OF VEHICLE SLIP An explanation of how sideslip occurs is necessary to understand the errors that can potentially arise in a navigation solution. The tire is the vehicle?s interface with the road which transfers all drive forces to the ground and generating the required lateral forces to turn the vehicle. Using the explanation presented in (Milliken, 1995), as the tire rolls, parts of the tread either adhere to the road or slide. When a force from the vehicle is applied to the tire/road interface, the static coefficient of friction is exceeded in local areas on the tread. Areas where the friction coefficient is surpassed begin to slide, which generates a force to push the vehicle. The generated force is approximated as linear, proportional to slip until the tire saturates. When saturation occurs, the force remains constant as no more lateral or longitudinal force can be applied to direct the vehicle. The linear increase and saturation can be seen in the tire curve below. -10 -5 0 5 10 -3000 -2000 -1000 0 1000 2000 3000 Tire Slip Angle (deg) La t e r a l Fo r c e ( N ) ?=0.8 ?=0.2 C ? Figure 4.1: Lateral tire curve saturating on different surfaces. The effective stiffness of the tire is used to characterize the tire, and is denoted by a cornering stiffness (C ? ) and tractive stiffness (C s ). These parameters and the peak tire force are functions of numerous factors such as terrain, normal force, toe, camber, road conditions, materials, etc, and are rarely known exactly due to the highly complex relationship between these functions. Approximations with look up tables or empirical models, such as Pacjeka?s Magic Model or the Dugoff Tire Model (Figure 3.4), are often used to sufficiently characterize the tire (Pacejka, 1987; Dugoff, 1970). Ultimately, the vehicle control forces in the linear region are a function of the tire stiffness and lateral and/or longitudinal slip (? and s, respectively), shown by Equation 4.1. ? ? ?= ?= CF sCF y sx 4.1 51 In the above equation, s is the slip due to the difference in wheel speed and vehicle speed, and ? is the slip due to the difference in the tire pointing direction and the direction of travel (Gillespie, 1992). Wheel slip can be calculated using a wheel encoder and a sensor to measure vehicle velocity, such as GPS, radar, or optical sensor. Definitions of longitudinal and lateral tire slip are provided in Equation 4.2. ? ? ? ? ? ? ? ? = ?? = ? xT yT v wV V V V RV s 1 tan? 4.2 The lateral slip at each tire can be translated to the vehicle?s center of gravity (CG) to obtain overall vehicle slip, more commonly known as vehicle sideslip (?). Sideslip is an angle defined as the difference between the vehicle?s course (?) and its heading (?). This difference can be seen in the figure below, which is typically referred to as a ?bicycle schematic.? 52 Figure 4.2: Bicycle schematic showing slip angles. Vehicle heading is the direction the vehicle is pointed, while course is the direction of the vector created by the vehicle?s direction of travel. Both of these angles are referenced clockwise from North. In practice, this angle is measured or estimated at the CG, then can be translated to other locations on the vehicle for analysis or to extract further information. In Figure 4.2, the slip angle at the CG was translated to each end of the vehicle to provide the tire slip angles. Model based Kalman filters have proven to be effective tools to estimate vehicle states but often require significant knowledge about the vehicle (Anderson, 2004). Kinematic relationships between sensors on a vehicle are often used to define state equations in a Kalman filter because they require less model knowledge and are not affected by to changes in the model, such as vehicle mass or tire parameters. However, these kinematic models sometimes depict the car as if it were on rails by assuming zero 53 lateral velocity, or by assuming a proportional relationship between steer angle and yaw rate. These assumptions are valid at low speeds for passenger vehicles as the car is generating minute slip angles and the lateral velocity is virtually zero. The assumptions break down at higher speeds as lateral velocities are generated that invalidate the kinematic model, as will be shown in this chapter. When this occurs a degradation of the performance of navigation or control systems may be observed. For these assumptions, speed also must be considered relative to the vehicle parameters, or the terrain on which the vehicle is operating. A small All Terrain Vehicle (ATV) with soft tires maneuvering at 20 m/s on pavement could generate the same slip angles as a sports coupe with performance tires traveling 35 m/s. Similarly, a car turning on dry pavement at 10 m/s would generate very small slip angles, while the same car traveling at the same speed would generate significantly larger slip angles if it were on a low friction surface such as ice. 4.2. NAVIGATION CORRUPTION BY SIDESLIP A simple but common estimator utilizes a single antenna GPS receiver and an IMU to track vehicle position, orientation, and speed. The GPS receiver measures the vehicle?s course, speed, and position, while the IMU measures the vehicle?s yaw rate and longitudinal acceleration. The Kalman filter compares the integrated yaw rate gyro with the course measurement, and the integrated accelerometer to the speed measurement, to estimate the vehicle?s speed, orientation, longitudinal accelerometer bias, and yaw rate gyro bias. The yaw rate gyro provides a measurement of the time derivative of heading, r=?& 4.3 54 and course is the combination of heading and sideslip. ??? += 4.4 Typically a heading measurement is unavailable, so the course measurement is substituted. Under straight or low speed driving, the integrated yaw rate, or heading, and course measurements should be similar. ??? ?? +???? ???= ? ? dtr dtr 0 0 & & 4.5 Similarly, small sideslip angles fulfill the small angle approximation, so vehicle speed can be approximated with an integrated longitudinal accelerometer. However, vehicle speed gains a lateral component when sideslip is non-zero. )sin()cos( ?? yx xx VVV dtaV += ?= ? v 4.6 Maneuvers producing sideslip, such as an emergency lane change, introduce a discrepancy in the yaw rate gyro bias estimate because heading from an integrated yaw rate gyro is compared to GPS course in the innovation in the EKF (Equation 2.11). However, GPS course is measuring the angle of the velocity vector (heading plus sideslip), and the heading is measuring the angle the vehicle is pointing. Further complications arise because the longitudinal accelerometer in the IMU is measuring a centripetal acceleration neglected in the models, and GPS velocity is measuring both longitudinal and lateral velocity. These neglected terms can lead to significant state estimate errors if a vehicle is generating even small sideslip angles. As will be seen in the ensuing subsections, kinematic relationships that account for sideslip should be 55 56 included in the state equations in the Kalman filter in order to produce a more accurate navigation solution. Three navigation models were utilized to examine the solution accuracy when vehicle sideslip is ignored and when it is taken into account. All algorithms use the EKF given by Equations 2.8 to 2.12. Noise statistics for the process covariance matrix (Q) were determined by calculating the standard deviation of experimental static sensor data. Since the data consisted of discrete samples, it required conversion to the time domain by a multiplication factor of the sample rate so it could be used in the EKF (Stengal, 1994). The statistics for the measurement covariance matrix (R) were taken from data sheets from the GPS receivers used. Biases were modeled as a random walk, and their respective entries in the Q matrix were used to tune the EKF. 4.2.1. Typical Navigation Model The most common states critical to a vehicle controller are speed and orientation. Additionally, accurate position is required for autonomous operation and future stability and control routines. A typical, computationally inexpensive navigation model on ground vehicles provides states of speed, orientation about the z axis, position, and sensor biases using inputs of longitudinal acceleration and yaw rate. Equation 4.7 displays a six state kinematic navigation model including the listed states. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = )?sin( ? )?cos( ? 0 ? 0 ? ? ? ? ? ? ? ? ? ? ? V V br ba E N b b V x r axx r ax & & & & & & & 4.7 The A matrix used in Equation 2.8 is computed as the Jacobian of this vector field with respect to the states evaluated at the previous time step. Note the simplified kinematic coupling of the vehicle states. The velocity estimate is assumed to be in the longitudinal direction of the vehicle, while the heading estimate lumps heading and course into one state. Measurements for this system consist of GPS velocity, course, and position. The measurement matrix is a 4x6 matrix of zeros unless a GPS message is received. The four entries linked to the respective states change to a value of 1 (Equation 4.8). )North (GPS ,1)5,3( Speed) (GPS ,1)1,1( = = C C East) GPS( 1)6,4( Course) (GPS ,1)3,2( = = C C 4.8 It is important to note the system is unobservable when GPS is not available, and estimates are calculated by dead reckoning with the IMU. 4.2.2. Modified Navigation Model A modified version of the model listed above estimates vehicle roll using a roll rate gyro and lateral accelerometer using methods defined by (Bevly, 2004). These sensors are common on some current passenger vehicles. An estimate of vehicle roll can be useful in stability control systems to provide additional vehicle information. It has been shown that vehicle roll angle is a function of CG location, not just height, and therefore 57 lateral acceleration is not always a sufficient indicator of vehicle instability (Whitehead, 2004; Travis, 2004). Inputs are longitudinal acceleration, yaw rate, and roll rate, resulting in the state equations for this system are listed below in Equation 4.9. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? + = )?sin( ? )?cos( ? 0 ? 0 ? 0 ? ? ? ? ?? ? ? ? ? ? ? ? ?? ? ? ? V V b br ba E N b b b b V x IMU r axx ay r ax ax & & & & & & & & & 4.9 Using an additional kinematic relationship, a vehicle roll measurement can be obtained by subtracting the centripetal acceleration from the lateral accelerometer measurement as show in Equation 4.10. ()) ? ( 1 ryM brVa g ??=? 4.10 Any lateral accelerometer bias will appear in this calculation; therefore the estimate is actually a combination of roll angle and lateral accelerometer bias. Bevly also proposed a method to separate the states by applying a low pass filter to the estimate to obtain the bias and applying a high pass filter to obtain the change in roll angle. The measurement matrix is a 5x8 matrix of zeros. When a GPS message is received, the following matrix entries change (Equation 4.11). 58 East) (GPS 1)8,5( )( ,1)5,3( Speed) (GPS ,1)1,1( M = = = C C C ? North) (GPS ,1)7,4( Course) (GPS ,1)3,2( = = C C 4.11 4.2.3. Expanded Navigation Model A navigation algorithm was derived to compensate for vehicle slip using kinematic relationships that are sometimes disregarded. Inputs are from sensors common to most current passenger vehicles: longitudinal acceleration, lateral acceleration, yaw rate, and roll rate. The vector field containing these kinematic equations is shown in Equation 4.12. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ++ ++ ? ? ???? ?+? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = )) ? / ? (atan?sin() ?? ( )) ? / ? (atan?cos() ?? ( 0 ? 0 ? 0 ) ? sin() ? ( ?? 0 ) ? ( ?? ? ? ? ? ? ? ? ? ? ? ? 2/122 2/122 xyyx xyyx IMU r rxayy ryaxx r ay y ax x VVVV VVVV b br gbrVba brVba E N b b b V b V x ? ? ? ? ? ? ? ? & & & & & & & & & & & & 4.12 59 The obvious difference between this modified model and the previously presented models is the velocity estimate is now broken up into longitudinal and lateral components, and additional centripetal acceleration terms are included. The main assumption of this model is there is no vertical velocity. This assumption is valid for this study because the vehicle was constrained to a level surface in simulation and in experimental tests. However, in scenarios where vertical velocities will be encountered (hills, banked turns, etc), more centripetal acceleration effects will appear in these equations and should be included in the navigation model to improve the estimation accuracy. GPS can provide the necessary measurements of vertical velocity, and pitch with a three antenna system, to make the navigation model observable. Measurements for this system are longitudinal and lateral velocity, heading (not course), roll, and position. Currently, many of these measurements are not available on passenger vehicles but are readily accessible on many AGV platforms. However, obtaining these measurements on a highway vehicle is not a distant concept as GPS technology continues to become more prevalent in automotive systems. Sideslip has been effectively measured using a yaw rate gyro and GPS, which is currently on many vehicles (Bevly, 2000). This study obtained a sideslip calculation from a two antenna GPS receiver. If sideslip is known, the GPS velocity measurement can be separated into longitudinal and lateral components for use in the state equations given in Equation 4.12. Discussion on how sideslip was obtained for this research is provided in Appendix B. Using the listed measurements, the C matrix is a 6x10 matrix of zeros unless a GPS measurement is received. In that instance, the measurement matrix is as follows: Heading) (GPS ,1)5,3( Speed) Lateral (GPS ,1)3,2( Speed) alLongitudin (GPS ,1)1,1( = = = C C C East) (GPS 1)10,6( North) (GPS ,1)9,5( Roll) (GPS ,1)7,4( = = = C C C 4.13 Also, an effective sideslip estimate can be calculated using V x and V y as follows: ? ? ? ? ? ? ? ? = ? x y V V 1 tan? 4.14 60 The process and measurement covariance matrices for the three models shared variances on common states. Also, the parameters did not change when using the models with simulated or experimental data, since the sensor simulation was tuned to match the actual sensors on the test bed. The values are listed in the tables below. Table 4.1: Process Noise Variances for Sideslip Corruption Investigation dt ax 22 2.0=? 42 1 ? = e bax ? dt ay 22 5.0=? 42 1 ? = e bay ? dt r 22 05.0=? 62 1 ? = e br ? dt 22 0107.0= ? ? 62 1 ? = e b? ? 62 1 ? = e N ? 62 1 ? = e E ? Table 4.2: Measurement Noise Variances for Sideslip Corruption Investigation 22 5.0= V ? 2 2 ? ? ? ? ? ? = V V ? ? ? 22 00174.0= ? ? 22 00698.0= ? ? 22 03.0= M? ? 22 1.0= N ? 22 1.0= E ? 4.2.4. Simulation Results of Sideslip Corruption A step lane change maneuver was input into the simulation discussed in Chapter 3 to excite the yaw dynamics. In the following plots, the numbers one, two, and three refer to the typical navigation model, modified model, and expanded model, respectively, listed in Section 4.2.1 to 4.2.3. The output of models 1 and 2 are often similar because of the similarity of their kinematic models (both neglect sideslip). 61 As seen in Figure 4.3, the heading estimates from models 1 and 2 lose accuracy when slip is generated. The estimate follows the heading measurement from the integrated yaw gyro until a GPS course measurement is received. The residual between the estimate and measurement becomes large because the model does not account for sideslip. This error is then multiplied by the EKF gain, and added to the time update to minimize the error between the estimate and the course measurement. This process occurs in Equation 2.11. Another casualty of this phenomenon is the yaw gyro bias estimate. Figure 4.4 shows error due to sideslip being injected in the yaw gyro bias estimate. However, when sideslip is taken into account by model 3, the severity of inaccuracies in the gyro bias estimate is reduced up to 1.5 deg/s for this maneuver. Additionally, the heading estimate correctly follows the true vehicle heading. 33 33.5 34 34.5 35 35.5 36 36.5 37 0 5 10 15 Time (s) Ya w E s t i ma t e ( d e g ) 1&2 3 True Course True Heading Figure 4.3: Heading estimates of the different navigation models from simulated data. 62 10 15 20 25 30 35 40 45 50 -0.5 0 0.5 1 1.5 2 Time (s) Ya w Ra t e Gy r o Bi a s ( d e g / s ) 1&2 3 True b r Figure 4.4: Yaw gyro bias estimates from simulated data. Comparable evidence is seen in the velocity and accelerometer bias estimates. When lateral velocity is near zero, the three models produce similar estimates. When slip is introduced, the longitudinal estimate using models 1 and 2 diverge from the estimates using 3. This is due to the fact that models 1 and 2 neglect the centripetal acceleration caused by lateral velocity and yaw rate. Figure 4.5 shows the error in the velocity estimate caused by the models that neglect centripetal acceleration. Figure 4.6 displays the inaccuracy in the longitudinal accelerometer bias estimate. 63 15 20 25 30 35 40 45 19.95 20 20.05 20.1 20.15 Time (s) S p eed E s t i m at e (m / s ) 1&2 3 True Figure 4.5: Jump in velocity estimate due to neglected terms. 20 25 30 35 40 45 -0.2 0 0.2 0.4 0.6 0.8 Time (s) A ccel er o m et er B i as (m / s 2 ) 1&2 3 ax 3 ay True b ax True b ay Figure 4.6: Corrupted longitudinal accelerometer bias estimate. 64 Although roll is not directly influence by sideslip, knowledge of this state is useful to vehicle controllers and could be used in algorithms to calculate other vehicle states. Figure 4.7 displays the roll estimate from models 2 and 3. The estimate from model 2 contains the lateral accelerometer bias from the measurement. However, model 3 correctly captures the roll angle with the use of the dual antenna GPS system. Note that a third antenna could be added to estimate pitch in a similar fashion. 20 25 30 35 40 45 50 -3 -2 -1 0 1 2 3 4 5 Time (s) R o ll (d e g ) 2 3 True Figure 4.7: Roll estimates from the different navigation models. Although the sideslip effects initially appear small, the loss of GPS during this period would devastate the estimate accuracy. There are two primary sources of error in models 1 and 2 during a GPS outage. Without GPS, the estimator integrates yaw rate to obtain a course measurement. However, this estimate is actually heading. Since the position estimates are a function of vehicle velocity and course, error will be introduced by using heading in lieu of course. Additionally, if the outage occurs while the vehicle is slipping, 65 the yaw gyro bias estimate will be incorrect and lead to a large position error. Model 3 overcomes the shortcomings of models 1 and 2 by continuing to estimate longitudinal and lateral velocity, which provides an effective sideslip estimate. The heading estimate from the integrated yaw gyro is added to the sideslip estimate, and used to estimate north and east position more accurately. Models 1 and 2 use only heading to estimate north and east position, which is incorrect when sideslip is present because north and east position are dependent upon direction of travel (course). Also, Figure 4.8 displays position error during both circumstances. The total error was calculated from the norm of east and north error as shown below, where True denotes the true value of the state and Est denotes the estimated value of the state. 22 )()( EstTrueEstTrueposition EENNe ?+?= 4.15 Using the same set of data as Figure 4.3 through Figure 4.7, a GPS outage was simulated before the maneuver was executed at 31 seconds (dashed line) and at 32.4 seconds when the vehicle was generating sideslip (dotted line). The models neglecting sideslip effects produced substantially larger errors when the outage occurred during the maneuver. Their accuracy was also poorer than model 3 when the outage occurred before the maneuver because vehicle heading, not course, was used to estimate north and east position when slip was generated. 66 0 10 20 30 40 50 0 10 20 30 40 50 60 70 Time (s) P o sitio n E r ro r (m ) 1&2 - Outage During Slip 1&2 - Outage Before Slip 3 - Outage During Slip 3 - Outage Before Slip Figure 4.8: Position error during an outage. Model 3 also displays some ability to predict sideslip during a GPS outage for a short period of time by integrating the lateral and longitudinal accelerometers. The model used the sideslip measurement from the dual antenna GPS receiver to estimate lateral and longitudinal velocity and correct inertial errors. This sideslip estimate would be of use to stability control systems in many vehicles. Figure 4.9 shows the sideslip estimate during the GPS outage using model 3. The growing error in the above figure was due to the bias drift in the accelerometers. 67 31 32 33 34 35 36 37 38 -6 -4 -2 0 2 4 Time (s) Si d e s l i p ( d e g ) During Maneuver Before Maneuver True Figure 4.9: Estimated sideslip during an outage. 4.2.5. Experimental Results of Sideslip Corruption Slip was generated experimentally to show the estimator accuracy using the various navigation models. An emergency lane change maneuver was executed on a straight section of the aforementioned test track. As in Section 0, models 1 and 2 refer to the typical and modified model, and model 3 refers to the expanded model. It should be noted the process noise statistics in Q can change during an experimental run as different levels of vibration are experienced throughout the vehicle. This matrix was initially set and not revisited. In simulation, this is sufficient because the process noise is constant. However in the real world, errors can be produced due to a change in the process noise model. These changes are hard to detect, therefore the process noise matrix is often left constant which leads to a sub-optimal filter. 68 Figure 4.10 shows the decrease in heading accuracy with GPS during the maneuver using models 1 and 2. Conversely, the estimator using model 3 correctly tracks the vehicle heading. This is similar to the simulation results shown in Figure 4.3. Notice the increase in the level of noise caused by an incorrect yaw gyro bias estimate from models 1 and 2. The Kalman filter could be tuned to decrease the severity of heading error by adjusting the appropriate bias parameters in the process noise covariance matrix. However, this might lead to an inability to accurately track the sensor bias, which would impact the dead reckoning solution. 197.5 198 198.5 199 199.5 -95 -90 -85 -80 -75 Time (s) Ya w Es t i ma t e ( d e g ) 1&2 3 True Course True Heading Figure 4.10: Heading estimates of the navigation models with experimental data. A GPS outage was artificially simulated at 118 seconds in Figure 4.11. During this period of time, sideslip was generated. Therefore this section of data was chosen to display the errors generated by the assumption of zero lateral velocity. The position error was calculated using Equation 4.15. Estimates from experimental data display similar 69 results as the estimates from the simulated data. Position error grows during the outage, but the model incorporating slip experiences a smaller error growth rate. 0 20 40 60 80 100 120 0 20 40 60 80 100 120 Time (s) P o sitio n E r ro r (m ) 1&2 3 Figure 4.11: Position error during an artificial GPS outage. Figure 4.12 provides insight as to a difference exists between the navigation models. When GPS is removed at 118 seconds, the yaw gyro bias is incorrectly estimated using models 1 and 2, which leads to an incorrect heading estimate. Model 3 captures the motion of the vehicle and provides a more accurate heading estimate for a short period of time during the outage. 70 116 117 118 119 120 121 122 123 75 80 85 90 95 Time (s) Ya w Es t i ma t e ( d e g ) 1&2 3 True Course True Heading Figure 4.12: Heading estimates during a GPS outage. 4.3. NAVIGATION CORRUPTION BY LONGITUDINAL SLIP Wheel speed sensors are perhaps the most common velocity sensor on ground vehicles. Their low cost, small size and ease of use are beneficial to manufacturers, and they are proven navigation instruments (Hay, 2005). However, an inherent flaw with a wheel speed sensor is that it measures wheel speed, not vehicle speed. When longitudinal slip is generated, these two measurements can drastically differ. Longitudinal wheel slip is defined by the speed difference between the vehicle and the tire, and is given by Equation 4.16. Vehicle VehicleTire V VV s ? = 4.16 Large engine torques can yield a high wheel speed measurement that differs from the vehicle?s speed over ground. Similarly, a hard braking maneuver could lock up a tire. 71 72 The slip is dependent upon tire characteristics, as explained in Section 4.1 and with Equation 4.1. The wheel speed sensor would then report zero velocity, but the vehicle might be traveling with a high rate of speed. Terrain only magnifies the difference between wheel speed and vehicle speed. A small slip percentage is typically generated by the drive torque at the tire. On ice, the same drive torque could produce a considerable percentage of wheel slip. In order to mitigate the effect of wheel slip, the wheel speed sensor is often placed on an un-driven wheel. To effectively use a wheel speed measurement for navigation, it must be translated from the tire to the vehicle?s center of gravity. If a wheel speed sensor on a front tire is used, an additional translation from the tire frame to the vehicle frame must be completed. As a vehicle turns, a velocity difference between inner and outer tires is introduced. Also, if sideslip is present, the wheel speed sensors on the tires cannot capture the lateral velocity. Figure 4.13 shows a front left tire at a given steer angle (?), along with distances to the CG. Figure 4.13: Tire coordinate frame. Using Equation 4.17 the wheel velocity can be moved to the CG with knowledge of the yaw rate. TCGTCG RrVV / ?+= vv 4.17 First, the wheel velocity should be separated into x and y components and translated to the vehicle coordinate frame. [ ] [ ] () ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??+ ++?= y f x y xLFyFL x yFLxFLFLCG t ar VVVVV v v vv v 2 )sin()cos()sin()cos( / ???? 4.18 Simplifying Equation 4.18 yields the complete velocity translation from the front left tire to the vehicle?s CG. [] y xLFyFL x f yFLxFLFLCG arVV r t VVV v v v +++ ? ? ? ? ? ? +?= )sin()cos( 2 )sin()cos( / ?? ?? 4.19 Following a similar procedure, the velocity at any tire can be moved to the CG. Equation 4.20 shows the transformation from the front right tire. 73 [] y xLFyFL x f yFLxFLFRCG arVV r t VVV v v v +++ ? ? ? ? ? ? ??= )sin()cos( 2 )sin()cos( / ?? ?? 4.20 In most circumstances rear tires do not require a tire-to-vehicle frame transformation. Therefore a lateral wheel velocity has no influence on the longitudinal vehicle velocity. Therefore, the velocity from the back left tire to the CG is as follows: [] y yBL x r xBLBLCG brVr t VV v v v ?+ ? ? ? ? ? ? += 2 / 4.21 Finally, the transformation from the rear right tire to the CG is calculated. [] y yBR x r xBRBRCG brVr t VV v v v ?+ ? ? ? ? ? ? ?= 2 / 4.22 4.3.1. Navigation Model with Wheel Speed The navigation system is simplified in this section by estimating states only pertaining to vehicle velocity. Therefore, the system nonlinearities induced by the position estimate are eliminated and a simpler, discrete linear Kalman filter (LKF) is used. The guaranteed optimality provided by a linear Kalman filter is a welcomed benefit. The LKF is similar in design to the EKF but the equations are slightly different because there are no nonlinear approximations. For more information, see Gelb (1974). The equations for an LKF are listed below. State Estimate Extrapolation 1111 ?? ?? + ?? ? += kkkkk uBxAx 4.23 Covariance Estimate Extrapolation 1111 ?? + ?? ? += k T kkkk QAPAP 4.24 74 Gain Calculation () 1? ?? += k T kkk T kkk RCPCCPL 4.25 State Update ( ) ??+ ?+= kkkkkk xCyLxx ??? 4.26 Covariance Update () 1? ?+ += kk T kkk CRCPP 4.27 Another simplification in this section is the use of a rear wheel speed measurement, as opposed to a front wheel speed measurement. This eliminates the lateral wheel velocity contribution to the longitudinal vehicle velocity. Also, tests are run in a manner to minimize sideslip so the lateral components can be neglected. After these simplifications, the velocity transformation from the left rear tire to the vehicle?s CG is given by Equation 4.28. r t VV r xBLBLCG 2 / += v 4.28 The navigation model to estimate longitudinal vehicle speed and longitudinal accelerometer bias is given in Equation 4.29. ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? = 0 ? ? ? ? axx ax x ba b V x & & & 4.29 The sole input is longitudinal acceleration. This leads to the continuous state transition matrix, A c , and continuous input matrix, B c , below. ? ? ? ? ? ? = ? ? ? ? ? ? ? = 0 1 , 00 10 cc BA 4.30 Measurements of GPS velocity and wheel speed translated to the CG yield the measurement vector and measurement matrix given by Equation 4.31. 75 ? ? ? ? ? ? = ? ? ? ? ? ? = 01 01 , / c BLCG GPS C V V y 4.31 Note that the continuous system matrices must be discretized before used in the LKF. The addition of GPS velocity allows a wheel speed bias estimate to be observable. Addition of a third state is required to estimate wheel speed bias in the above system. ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 0 0 ? ? ? ? ? axx ws ax x ba b b V x & & & & 4.32 The state matrices when estimating a wheel speed bias are defined as follows: ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? = 101 001 , 0 0 1 , 000 000 010 ccc CBA 4.33 Note the same input and measurements are used for both models. 4.3.2. Navigation Model with Wheel Speed and Radar Doppler radar is a common speed sensor which can be utilized on ground vehicles. It is helpful on some terrains because it is insensitive to wheel slip. The radar bias however may be sensitive to changes in the sensor?s pointing angle and can be terrain dependent. Therefore a bias associated with the radar should be estimated to provide an accurate velocity estimate. The navigation model used is similar to the model estimating wheel slip, with an additional state to estimate the Doppler radar bias. 76 ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 0 0 0 1 , 0000 0000 0000 0010 , 0 0 0 ? ? ? ? ? ? cc axx dop ws ax x BA ba b b b V x & & & & & 4.34 The measurement vector is altered by the addition of the Doppler radar velocity measurement. It is listed with the measurement matrix in Equation 4.35. ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? = 1001 0101 0001 , / c dop BLCG GPS C V V V y 4.35 4.3.3. Experimental Results of Longitudinal Slip Corruption The Infiniti G35 was driven in a straight line on a gravel surface to show wheel slip in the longitudinal direction can be devastating to the accuracy of the navigation solution when using a wheel speed sensor to provide a redundant measurement of velocity. Figure 4.14 displays the corrupted navigation solution when wheel slip is not accounted for in the estimator while GPS is available. At 4.5 seconds there is a 1 m/s error in the velocity estimate due to wheel slip. Recall that wheel slip is related to applied drive force at the wheel, and therefore related to acceleration. 77 0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 Time (s) V e lo c ity (m /s ) Wheel Speed GPS Estimate Figure 4.14: Velocity estimate corrupted by wheel slip. Estimating the wheel speed bias effectively lessens the velocity estimate error when GPS is available, as shown by Figure 4.15. The longitudinal slip is captured in the bias estimate if the bias is modeled with high enough dynamics, along with any radial displacements caused by flexing of the sidewall as the tire is loaded during operation. 78 0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 Time (s) V e lo c ity (m /s ) Wheel Speed GPS Estimate Figure 4.15: Estimate with wheel speed bias removed. When GPS is lost the bias estimate becomes unobservable and remains constant. Figure 4.16 shows the same data given in Figure 4.14. In this figure, a GPS outage is simulated at t=4 seconds. The effect of the bias remaining constant can be seen. 79 0 5 10 15 20 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (s) W h ee l S p eed B i a s (m / s ) Figure 4.16: Estimated wheel speed bias before and during a GPS outage. Because the wheel speed bias is unobservable when GPS measurements are unavailable, any changes in the bias cannot be compensated. The dead reckoned navigation solution is corrupted by changes in slip or wheel radius during a GPS outage. Figure 4.17 shows the error in the velocity estimate is proportional to the constant bias offset. 80 0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 Time (s) V e lo c ity (m /s ) Wheel Speed GPS Estimate Figure 4.17: Velocity estimate using wheel speed and bias estimate during an outage. The Doppler radar velocity measurement was used to correct the inadequacies of the wheel speed measurement using the estimator model described in Equations 4.34 and 4.35. Figure 4.18 shows the same data with a velocity estimate using wheel speed and radar measurements. It initially appears the radar measurement is the solution to accurately measuring ground vehicle speed. The corruption due to longitudinal wheel slip is subdued resulting in a higher precision velocity estimate. However, the radar bias soon degrades the estimate as well, which is seen below. 81 0 2 4 6 8 10 12 14 16 0 2 4 6 8 10 12 Time (s) V e lo c ity (m /s ) Radar Wheel Speed GPS Estimate Figure 4.18: Estimated vehicle velocity with Doppler radar. The radar bias is not constant enough for it to be considered an ideal measurement. The speed measurement is a function of the sensor?s distance and angle relative to the ground. Therefore, the Doppler radar output is affected when the vehicle rolls or pitches due to acceleration or road grade because the distance and angle of the unit changes relative to the ground. Surface conditions also impact the Doppler radar output because the height and texture of different terrains alter the reflectivity of the microwave signal. Figure 4.19 is a plot showing the radar bias on different terrain. 82 Figure 4.19: Doppler radar bias on different terrain. 4.4. SUMMARY A demonstration of navigational accuracy was performed utilizing a nonlinear vehicle simulation to provide a controlled environment and experimental data to emulate an actual scenario. The accuracy of the navigation estimates was improved with and without GPS by accounting for the lateral velocity generated when a vehicle turns. The model developed to include sideslip effects tracked the vehicle?s velocity and heading more accurately than traditional models that typically neglect the lateral dynamics. The model also better compensated the inherent sensor biases which directly lead to a dead reckoning solution with less error. In the longitudinal direction, scenarios were presented that expose the shortcomings of typical dead reckoning sensors. A wheel speed sensor can aid in estimating velocity 83 84 when the wheel slip is taken into account, and can handle GPS outages well if slip remains constant. When slip varies, the bias estimate becomes corrupted and increases the position error. Similarly, a Doppler radar can provide adequate ground speed measurements during a GPS outage unless the terrain on which the vehicle is traveling changes. The radar speed bias is a function of terrain, and is not observable during an outage. 85 5. INS AIDING WITH A LIDAR Autonomous capability and vehicle control systems require reliable and robust navigation solutions in multiple environments. GPS has become an effective tool but is not suitable for all environments. Satellites are not always in direct view of the GPS receiver, which is hindering the development of reliable AGVs. Environmental constraints can either completely block the signal, or degrade the signal to the point where it is of no use. Jamming can cause similar issues in combat environments, where the opposition can cheaply and effectively knock out the GPS signals or broadcast incorrect messages. If the GPS signal is suddenly lost, jammed, or known to be incorrect, some critical system states become unobservable without additional sensors. In these circumstances, the navigation solution generally relies on integrating noisy, biased IMU measurements, affecting navigation accuracy and control capabilities. Laser scanners are quickly making their presence known in the navigation field and are proven to have a variety of uses (Lei, 2005). In this chapter, a light detection and ranging device (Lidar) is used in conjunction with data from a MEMS based IMU to formulate a suitable navigation solution (Travis, 2005). Lidar is has been used as a navigation tool on both aerial (Campbell, 2003) and ground vehicles (Hirokawa, 2004; Talaya, 2004). Specifically, this chapter examines the operation of a ground vehicle in an indoor corridor environment. The use of a Lidar compensates the lack of GPS by providing the following measurements of critical vehicle states: heading, velocity, and lateral error (Hirokawa, 2004). This type of navigation could be extended to many environments, such as an urban canyon, forest, indoor corridor, or in jammed environments. 5.1. TEST APPARATUS 5.1.1. Experimental Setup A cart was outfitted with an IMU and Lidar. The cart had four independent wheels that were free to rotate about the x and z axes of a vehicle coordinate frame (x being forward, z being down). It was manually pushed down a corridor at different velocities while data was logged from the IMU and Lidar. The absence of an engine or motor to translate the cart reduced the amount of vibration that the sensitive IMU would measure. The majority of the noise stemmed from a fan on a power supply and irregularities along the ground where the cart was rolled. Data from each run was passed through a Kalman filter, post process, to develop estimates of velocity, heading, and lateral error. 5.1.2. IMU The IMU used in this analysis was a Crossbow IMU400CC-100, capable of providing 3 axis acceleration and angular velocity measurements. The IMU utilizes MEMS based accelerometers and rate gyroscopes, which have inherent walking biases and scale factor errors. The Crossbow output was recorded at 100Hz. Two scenarios are covered in this chapter and require two different input matrices. The first input set contains longitudinal acceleration and yaw rate as given in Equation 5.1, ? ? ? ? ? ? = r a u x 1 5.1 and the second input set adds lateral acceleration as shown in Equation 5.2. 86 ? ? ? ? ? ? ? ? ? ? = y x a r a u 2 5.2 5.1.3. Lidar Lidar is used to ?visually? access the environment and extract information, to update the IMU data. The SICK LMS 221 Lidar projects infrared laser pulses into the local environment and records their ?time of flight? and reflected intensity at 180 scanning points over the range 0? to 180? at a rate of 13.32 milliseconds per scan. The scanning points and the associated distances give a 2D image of the environment in a polar coordinate position and distance at each time step (? k , d ?k ). Using these measurements, a local heading, velocity, and lateral position from a defined line, y lat , are found. The measurement vector is as follows: ? ? ? ? ? ? ? ? ? ? = m lat m m m y V y ? 5.3 The Lidar?s heading estimate ? m is found as a function of the center of the hallway. A hallway?s center can be estimated by halving the distance between the corners of the hallway. Each corner in a hallway has the unique distinction of an edge. This is due to the fact the change in distance for a given set of data points (?, d ? ) and (? +??, d ? +?? ) located at a corner is large, as shown in Figure 5.1. These corners are determined as spikes in the derivative of the scanned points. 87 Figure 5.1: Corner detection with a Lidar. Once the center of the hallway is determined, the ? m estimate is determined as the angle between the 90? centerline of the Lidar and the center of the hallway as seen in Figure 5.2. Additionally, the lateral position of the Lidar is found as a distance from the hallway?s center. Two distances i d ,? are determined at ? = ?60? angles from the center of the hallway. These distances are used to determine the lateral distance from the Lidar to the edge of the wall y lat,i . 88 Figure 5.2: Calculating lateral offset. All of the Lidar?s data was run through a 10 point moving average filter to minimize the effect of any spikes occurring in the measurement from the detection of a false obstacle. The tests were run in a fairly controlled environment with smooth corridors, minus occasional doorways and small, miscellaneous objects. Other types of corridors, such as rows of trees lining a road, will be less consistent and generate more data spikes. The moving average filter aids in smoothing out the corridors and provides the Kalman filter a more discernible measurement. 5.2. NAVIGATION MODELS To thoroughly investigate the capabilities of utilizing Lidar for vehicle navigation, two kinematic models were derived. The first mathematical analysis assumes zero vehicle sideslip, so the discrepancy between vehicle heading and course is due only to error and not to a lateral velocity. The following five states are estimated in the first 89 scenario: velocity, longitudinal accelerometer bias, heading, yaw gyro bias, and lateral error. Inputs to the system from the IMU are stated in u 1 of Equation 5.1 and measurements are provided by the Lidar as shown in Equation 5.3. Note that the component for lateral error, , is a lateral velocity in the environmental coordinate frame, not the vehicle coordinate frame. There is zero lateral velocity in the vehicle coordinate system due to the zero sideslip assumption. Therefore, the state derivative vector is defined by the following: x& lat y& ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = )?sin( ? 0 ? 0 ? ? ? ? ? ? ? 1 ? ? x r axx lat r ax x V br ba y b b V x & & & & & & 5.4 The Jacobian of this model yields the A matrix used in Equation 2.8. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 00)?cos( ? 0)?sin( 00000 01000 00000 00010 ?? V A 5.5 The measurement matrix corresponding to the measurements presented in Equation 5.3 is as follows: ? ? ? ? ? ? ? ? ? ? = 10000 00100 00001 C 5.6 In some instances, the sideslip can be used as an analysis tool or as a focal point of control and merits further investigation. Therefore, a second kinematic model was 90 developed that acknowledges that the vehicle can translate laterally within its own coordinate frame, creating an additional lateral acceleration component. This model uses the same measurements from the Lidar but couples them with the inputs from u 2 in Equation 5.2. For this scenario, the sixth estimated state is lateral velocity in the vehicle frame, as shown below. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? + ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = y yx r axx y lat r ax x a VV br ba V y b b V x )?cos( ? )?sin( ? 0 ? 0 ? ? ? ? ? ? ? ? 2 ?? ? & & & & & & & 5.7 The continuous state matrix is the Jacobian of the nonlinear state derivative vector. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 000000 )?cos(00)?sin( ? )?cos( ? 0)?sin( 000000 001000 000000 000010 ???? yx VV A 5.8 Equation 5.9 displays the measurement matrix when all measurements are available. ? ? ? ? ? ? ? ? ? ? = 010000 000100 000001 C 5.9 91 Input variances were obtained by recording static data, and were then placed along the diagonal of the process covariance matrix (Q). Biases are assumed to be constant in the system model, but they were modeled in the process covariance matrix to prevent the Kalman filter gains from going to zero. If this were to happen, the bias states would remain constant when they change in reality. The first scenario used a process covariance matrix of the form shown in Equation 5.10. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 2 2 2 2 2 0000 0000 0000 0000 0000 ylat br r bax ax Q ? ? ? ? ? 5.10 The process noise covariance matrix for the second scenario adds a variance for the lateral velocity state and is shown below. ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 2 2 2 2 2 2 00000 00000 00000 00000 00000 00000 ay ylat br r bax ax Q ? ? ? ? ? ? 5.11 The process noise and measurement noise statistics between the two models were shared. The values used are listed in the tables below. Table 5.1: Process Variance Values for Lidar/INS Aiding dt ax 22 032.0=? 42 1 ? = e bax ? dt r 22 0029.0=? 62 1 ? = e br ? dtdt ylat 222 1.0 ,01.0 ,0=? dt ay 22 0534.0=? Table 5.2: Measurement Variance Values for Lidar/INS Aiding 22 028.0= V ? 22 001.0= ? ? 22 1 02.0= ylat ? 92 Total lateral movement is a function of V x , V y , and ?. In the first scenario neglecting sideslip, the standard deviation in the lateral error is already accounted for in the ? ax and ? r terms in the Q matrix. In theory, ? ylat , should be set to zero to refrain from injecting unnecessary process noise into the system. This parameter, ? ylat , essentially can become a tuning knob to attempt to estimate unmodeled disturbances. The process noise on the lateral error state is non-Gaussian because of unmodeled dynamics, which technically makes the Kalman filter sub-optimal. However, but the effects are minimal in many situations including the one investigated in this chapter. In the second scenario, ? ax and ? r again account for some of the process noise in y lat , but there is additional process noise due to the lateral acceleration measurement incorporated into the system. As it turns out, accounting for the process noise when estimating V y automatically injects the process noise into the lateral error state. Because , the disturbances modeled with ? ) ? , ? (? yxlat VVfy = ay are included in the lateral error estimate through the lateral velocity estimate. Therefore the process noise on the lateral error state, ? ylat , may remain zero. Together, these statistics form the continuous process covariance matrix for both scenarios. Measurement noise statistics were calculated by averaging multiple static data sets taken along the testing path. For the Lidar, noise is sufficiently estimated by examining a static, or stationary, set of data at the beginning, middle, and end of the hall. For a more thorough examination of the noise, techniques in detection and estimation could be used to determine more appropriate noise characteristics (Adams, 2000). The lateral accelerometer measurement was assumed to be unbiased for the second scenario. The mean of the lateral acceleration data was removed before input into the 93 Kalman filter. With a measurement of lateral distance to the corridor in the vehicle frame (shown in Figure 5.3), lateral velocity and lateral accelerometer bias is observable. y dr , 0? y dl , 0? y lat1 y lat2 ? 1 ?? 2 ? Figure 5.3: Lateral distance measurement. This lateral distance measurement could be incorporated into the proposed algorithm from Section 4.2.3 to more accurately navigate with or without GPS. To do so, the following model must be incorporated: ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? ? = V ay V V ayy y b y x y ba x ? ? ? , ? 0 ? ? & & & 5.12 The Jacobian of this model is given in Equation 5.13. ? ? ? ? ? ? ? ? ? ? ? = 001 000 010 A 5.13 The measurement vector and measurement matrix are defined as follows: 94 95 ][] [ 100 , 1 == Cyy d This lateral distance measurement was not available at the time of this study. However, the proposed method could be used to determine vehicle lateral velocity and sideslip if the proper measurements are provided. Typically a Lidar can offer this measurement if set up to do so. 5.3. EXPERIMENTAL RESULTS 5.3.1. Scenario 1 ? Five State Estimator As stated earlier, scenario one investigates the use of Lidar in vehicle navigation when the vehicle?s lateral velocity component is assumed to be zero. As shown in Figure 5.4 and Figure 5.5, the Kalman filter algorithm using the Lidar measurements accurately estimates velocity and heading states. 0 5 10 15 20 -0.5 0 0.5 1 1.5 2 Time (s) V e lo c i t y ( m /s ) Lidar ?a x KF Figure 5.4: Velocity estimate using a Lidar. 0 5 10 15 20 -2 -1.5 -1 -0.5 0 0.5 1 Time (s) He a d i n g ( d e g ) Lidar ?r KF Figure 5.5: Heading estimate using a Lidar. 96 The heading estimate in Figure 5.5 captures the high dynamics as measured by the IMU during short durations, but follows the Lidar measurement long term to remove the effect caused by biases in the system. Lateral error in Figure 5.6 quickly tracks an initial offset of 0.25 meters, but is slow to respond to other lateral changes. This is due to over filtering because the lateral error covariance was set to zero. Subsequent results will show the level of filtering can be adjusted to improve the estimate by changing the lateral error covariance value or by including the lateral vehicle dynamics. 0 5 10 15 20 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 Time (s) L a te r a l E r r o r , y la t (m) Lidar KF Figure 5.6: Lateral error estimated using an IMU and Lidar. Figure 5.7 and Figure 5.8 show the longitudinal accelerometer bias and yaw rate gyro bias, respectively. As seen in the figures, the biases are estimated accurately as they jump to an initial offset then vary appropriately to capture the slow bias drift. 97 0 5 10 15 20 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 L o n g i t u d in a l A c c e le r o m e te r B i a s ( m /s 2 ) Time (s) Figure 5.7: Estimated longitudinal accelerometer bias. 0 5 10 15 20 -0.15 -0.1 -0.05 0 0.05 0.1 Y a w Gy r o B i a s ( d e g / s ) Time (s) Figure 5.8 Estimate of the yaw rate gyro bias. 98 To investigate the slow tracking of the lateral error estimation, the ? ylat ?knob? was tweaked to account for unmodeled disturbances or dynamics in the system. Discrete values of 0.01 and 0.1 meters were inserted into the process covariance matrix. This value has minimal affect on the velocity, heading, and bias estimates, but it has an obvious affect on the lateral error estimate. Figure 5.9 and Figure 5.10 show the lateral position estimates with the low and high process noise values. As ? ylat increases, the estimate converges on the Lidar measurement and provides less filtering. 0 5 10 15 20 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 Time (s) L a te r a l E r r o r , y la t (m) Lidar KF Figure 5.9: Lateral error estimate assuming no lateral velocity and ? ylat =0.01. 99 0 5 10 15 20 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 Time (s) L a te r a l E r r o r , y la t (m) Lidar KF Figure 5.10: Lateral error estimate assuming no lateral velocity and ? ylat =0.1. 5.3.2. Scenario 2 ? Six State Estimator The second scenario investigated the effectiveness of navigation with Lidar when a lateral acceleration measurement is available. Often this measurement is corrupted by process noise or vehicle dynamics, such as vibration or roll, or there are no suitable measurements to make the system observable. The test bed did not contain many of these additional sources of error so the lateral accelerometer recorded fairly clean data. There was no lateral velocity sensor on board which made lateral velocity and lateral accelerometer bias unobservable. Bias was removed post process and assumed constant. This is a safe assumption because of the short duration of each test. The additional input has minimal affect on the velocity and heading states, shown in Figure 5.11 and Figure 5.12. However the lateral error estimate, shown in Figure 5.13, is significantly improved with the use of a lateral accelerometer. 100 0 5 10 15 20 -0.5 0 0.5 1 1.5 2 Time (s) V e lo c i t y ( m /s ) Lidar ?a x KF Figure 5.11: Velocity estimated with second navigation model. 0 5 10 15 20 -2 -1.5 -1 -0.5 0 0.5 1 Time (s) He a d i n g ( d e g ) Lidar ?r KF Figure 5.12: Scenario 2 heading estimate. 101 0 5 10 15 20 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 Time (s) L a te r a l E r r o r , y la t (m) Lidar KF Figure 5.13: Lateral error estimate using lateral acceleration input. 5.4. SUMMARY A method using a Lidar to detect the edges of a defined corridor and process the information in a Kalman filter to produce a navigation solution was proposed. The idea is not limited to only a laser scanner and can be used with any environmental sensor capable of providing distance and angle measurements to an object. One navigation model was developed to estimate lateral error, while another was developed to estimate lateral error and lateral vehicle velocity using a lateral accelerometer input. The level of filtering was adjusted by tuning covariance parameters. It was shown the use of a lateral accelerometer improved the estimate without requiring any modeling of unknown disturbances. 102 103 6. CONCLUSION 6.1. SUMMARY This thesis has shown the importance of including sideslip in a kinematic navigation model when concerned with high accuracy navigation. Sideslip induces estimate errors in states critical to vehicle controllers, such as heading and velocity. Degradation in the position estimate was observed as well. Future vehicle control systems will require more a precise navigation solution to operate correctly, therefore sideslip must be accounted for when navigating. Work presented in this thesis also examined the use of a laser scanner with an IMU to provide the vehicle with a lateral path error and lateral velocity estimate. The lateral error and lateral could be utilized by ground vehicle control systems to achieve better performance. The lateral velocity estimate could also be integrated into a navigation system to reduce navigation errors due to sideslip. The DARPA Grand Challenge provided the opportunity to develop a navigation system for an autonomous vehicle. An estimation algorithm was written using an extended Kalman-Bucy filter to provide an accurate navigation solution with and without GPS, as well as vehicle state information, to the various vehicle subsystems. The algorithm performed well while the vehicle was in competition, surviving GPS outages lasting four minutes at the National Qualification Event. A hardware failure halted the vehicle in the Grand Challenge before meeting any obstacles that would stress the navigation system. However, the system performed exactly as designed during the event. 104 While testing the navigation system for the AGV, it was noticed that position error grew when the vehicle took corners at high speeds. The soft tires of the AGV permitted tire slip as the vehicle accelerated laterally. It was hypothesized the vehicle dynamics were corrupting the navigation solution, due to the fact that vehicle sideslip creates a discrepancy within the Kalman filter between the integrated yaw rate gyro input and the GPS course measurement. When sideslip exists, these two angles are not equal. It was concluded this phenomenon warranted further investigation. To examine the navigation accuracy in the presence of sideslip, a five degree of freedom, nonlinear vehicle model was derived to determine vehicle state information with inputs of steer angle and velocity. The vehicle model was implemented in a MATLAB? simulation so the response of various driving maneuvers could be studied in a controlled environment. The simulation output was validated by comparing it to experimental data collected on an Infiniti G35. Once the simulation?s validity was confirmed, its output, along with experimental data, was used to show the errors stemming from vehicle sideslip by comparing typical navigation models with a model that accounted for the lateral vehicle dynamics. Kinematic navigation models were used to estimate vehicle states important to control systems. No vehicle model parameter knowledge was necessary to develop reliable navigation solutions. Sideslip was found with the use of a dual antenna GPS receiver, which only required simple geometric information about the vehicle to translate the slip angle to different points on the vehicle. The two antenna system also provided a roll measurement to estimate the roll dynamics. Roll is a critical component of the expanded model as it leads to a more accurate and robust lateral velocity estimate. 105 Effects from longitudinal slip were also studied. Wheel speed sensors can be used to provide a redundant velocity measurement to improve dead reckoning accuracy. However, in the presence of wheel slip the performance of the system with wheel speed sensors was shown to degrade. Doppler radar was used to complement the wheel speed measurement, but it was shown the radar measurement bias, although not susceptible to wheel slip, can be terrain dependent. It was shown in scenarios where terrain remains fairly constant a Doppler radar can provide an accurate velocity measurement during a GPS outage. Although the effects of dead reckoning errors can be reduced, they are not removed. Therefore, a navigation system incorporating a laser scanner was developed investigate its performance in a GPS denied environment. A Lidar/INS solution was shown to provide accurate state estimates for vehicle navigation within a defined corridor. The reliable measurements and statistical properties of the Lidar form a suitable companion for an IMU, similar to GPS, when using a Kalman filter to calculate a solution. Inertial errors due to integration of noise and bias are removed, while providing high update rate estimates of the dynamic vehicle states. A proposed method of tuning the performance of the Kalman filter has been offered by means of varying a process noise statistic on lateral velocity to account for unmodeled dynamics and/or disturbances to the system. It was shown how a lateral velocity estimate could be extracted to calculate sideslip, and for use in the proposed navigation model incorporating the lateral vehicle dynamics. A significance of this research is it does not rely on GPS, and therefore could be used in urban environments, in situations where GPS signal is jammed, or in other GPS denied environments. 106 6.2. FUTURE WORK 6.2.1. Autonomous Navigation System Development As the autonomous vehicle market continues to expand, there will be a push to navigate cheaply, reliably, and robustly. To fulfill these demands, a tightly coupled approach should be investigated, possibly using a cascaded approach. Tightly coupled navigation algorithms are less vulnerable to signal loss, and still maintain limited navigation capability when fewer than four satellites are visible to the GPS antenna. As AGVs begin operating in more environments, this approach can provide significant advantages, as not all environments are favorable to GPS. Cascading the navigation model breaks up large matrices into multiple smaller ones. The computation expense is decreased exponentially, which means the algorithms could run on slower, cheaper machines. 6.2.2. Navigation Corruption by Vehicle Dynamics This study presented analysis of the errors arising from longitudinal and lateral vehicle slip. Throughout this thesis, the total vehicle slip was kept decoupled. Therefore it was sliding purely longitudinally or laterally, not both. Future work should analyze the potential error when longitudinal and lateral slip occur simultaneously. For instance, a vehicle on ice could be translating sideways with the tires locked up. Vehicle pitch dynamics should be included in the navigation model to study the improvement in estimated states. This thesis showed the improvement of including a centripetal acceleration term which was a function of the vehicle?s yaw rate. However, there are still centripetal accelerations due to roll and pitch rate that were not accounted for. Although the yaw rate is commonly much higher than the roll or pitch rate (therefore 107 its centripetal acceleration term is larger), some accuracy should be gained by adding the other effects. The addition of a third GPS antenna would provide the necessary means to make vehicle pitch observable in a navigation system. 6.2.3. Lidar/INS Navigation This preliminary study of Lidar as a navigation aid has provided a foundation for more extensive research. The next step is to try similar tests in different environments. Ideally, defined corridors should consist of road edges, but other objects along the side of the road may suffice. A SLAM based approach might be used to localize the obstacles and define a corridor, especially if the objects are widely spaced. The use other environmental sensors should be investigated as well. Auto manufacturers are beginning to include vision systems and laser scanners in vehicles. The presented research could be expanded to function on an automobile, where the estimates are used in a navigation system, stability system, or lane keeping control system. 108 REFERENCES 1. Adams, M.D., ?Lidar Design, Use, and Calibration Concepts for Correct Environmental Detection,? IEEE Transactions on Robotics and Automation, 16(6), December 2000, pp 753-761. 2. Alban, S., ?Design and Performance of a Robust GPS/INS Attitude System for Automobile Applications,? Thesis (PhD), Stanford University, Department of Aeronautics and Astronautics, 2004. 3. Anderson, R., ?Using GPS for Model Based Estimation of Critical Vehicle States and Parameters,? Thesis (MS), Auburn University, Department of Mechanical Engineering, 2004. 4. Arnold, J., ?New Applications Make NDGPS More Pervasive? Public Roads, 64(4), 2001. 5. Behringer, R., Sundareswaran, S., Gregory, B., Elsley, R., Addison, B., Guthmiller, W., Daily, R., Bevly, D.M., ?The DARPA Grand Challenge - Development of an Autonomous Vehicle,? The 2004 IEEE Intelligent Vehicles Symposium, June 2004, pp 226-231. 6. Behringer, R., Travis, W., Daily, R., Bevly, D.M., Kubinger, W., Herzner, W., Fehlberg, V., ?RASCAL ? An Autonomous Ground Vehicle for Desert Driving in the 109 DARPA Grand Challenge 2005,? Proceedings of the 8 th International IEEE ITSC Conference, September 2005, pp 644-649. 7. Bevly, D.M., ?Global Positioning System (GPS): A Low Cost Velocity Sensor for Correcting Inertial Sensor Errors on Ground Vehicles,? Journal of Dynamic Systems, Measurement, and Control, 126, June 2004, pp 1-10. 8. Bevly, D.M., Gerdes, J., Wilson, C., Zhang, G., 2000 ?The Use of GPS Based Velocity Measurements for Improved Vehicle State Information,? Proceedings of the American Controls Conference, 2000. 9. Campbell, J., de Haag, M.U., van Graas, F., ?Light Detection and Ranging-Based Terrain Navigation ? A Concept Exploration,? Proceedings of the ION GNSS 2003, September 2003 pp 461-469. 10. Cook, B., ?The United States Nationwide Differential Global Positioning System,? Proceedings of the ION National Technical Meeting 2000, January 2000, pp 297-305. 11. Daily, R., Bevly, D.M., ?The Use of GPS for Vehicle Stability Control Systems,? IEEE Transactions on Industrial Electronics, 51 (2), 2004, pp 270-277. 12. Dissanayake, G., Sukkarieh, S., Nebot, E., Durrant-Whyte, H., ?The Aiding of a Low- Cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications, ?IEEE Transactions on Robotics and Automation,? 17(5), October 2001, pp 731-747. 13. Dixon, J., ?Tires, Suspension, and Handling,? Society of Automotive Engineers, Warrendale, PA, 1996, ISBN: 1-56091-831-4. 110 14. Dugoff, H., Fancher, P.S., Segel, L., ?An Analysis of Tire Traction Properties and Their Influence on Vehicle Dynamic Performance,? SAE Paper 700377, 1970, pp 1219-1243. 15. Federal Highway Administration, ?High Accuracy ? Nationwide Global Positioning System Test and Analysys: Phase II Report,? McLean, VA: US Department of Transportation (FHWA-HRT-05-034), 2005. 16. Gebre-Egziabher, D., ?Design and Performance Analysis of a Low Cost Aided Dead Reckoning Navigator,? Thesis (PhD), Stanford University, Department of Aeronautics and Astronautics, 2004. 17. Gelb, A., ?Applied Optimal Estimation,? The MIT Press, Cambridge, MA, 1974, ISBN: 0-262-57048-3. 18. Gillespie, T., ?Fundamentals of Vehicle Dynamics,? Society of Automotive Engineers, Warrendale, PA, 1992, ISBN: 1-56091-199-9. 19. Godha, S., Cannon, M. ?Integration of DGPS with a Low Cost MEMS ? Based IMU for Land Vehicle Navigation Application,? Proceedings of the ION GNSS 2005, September 2005. 20. Guivant, J., Nebot, E., Baiker, S., ?Autonomous Navigation and Map Building Using Laser Range Sensors in Outdoor Applications,? Journal of Robotic Systems, 17(10), October 2000, pp 565-583. 21. Hay, C., ?Turn, Turn, Turn: Wheel Speed Dead Reckoning for Vehicle Navigation,? GPS World, October 2005, pp 37-42. 111 22. He, B., Wang, D., Pham, M., Yu, T., ?GPS/Encoder Based Precise Navigation for a 4WS Mobile Robot,? Seventh International Conference on Control, Automation, Robotics, and Vision (ICARCV), 2002. 23. Hirokawa, R., Nakakuki, K., Sato, K., Ishihara, R., ?Autonomous Vehicle Navigation with Carrier Phase DGPS and Laser-Scanner Augmentation,? Proceedings of the ION GNSS 2004, September 2004, pp 1115-1123. 24. Hirokawa, R., Nakakuki, K., Sato, K., Ishihara, R., ?Threading the Maze: GPS/INS, Landmark Sensing, and Obstacle Avoidance,? GPS World, 15(11), November 2004, pp 20-26. 25. Julier, S., Durrant-Whyte, H., ?On the Role of Process Models in Autonomous Land Vehicle Navigation Systems,? IEEE Transactions on Robotics and Automation. 19(1), 2003, pp 1-14. 26. Kogler, J., Hemetsberger, H., Alefs, B., Kubinger, W., Travis, W., ?Embedded Stereo Vision System for Intelligent Autonomous Vehicles,? Proceedings of the IEEE Intelligent Vehicles Conference, June 2006. 27. Lei, Y., Lu, G., ?Estimation of a Vehicle?s Position and Orientation Using Probabilistic Data Association,? Proceedings of the IMECE 2005, November 2005. 28. Milliken D., Milliken, W., ?Race Car Vehicle Dynamics,? Society of Automotive Engineers, Warrendale, PA, 1995, ISBN: 1-56091-526-9. 29. Olson, B., Shaw, S., St?p?n, G., ?Nonlinear Dynamics of Vehicle Traction,? Vehicle System Dynamics, 40(6), 2003, pp 377-399. 112 30. Ozguner, U., Redmill, K.A., Broggi, A., ?Team TerraMax and the DARPA Grand Challenge: A General Overview,? The 2004 IEEE Intelligent Vehicles Symposium, June 2004, pp 232-237. 31. Pacejka, H., Bakker, E., Nyborg, L., ?Tyre Modelling for Use in Vehicle Dynamics Studies,? SAE Paper 870421, 1987. 32. Scheding, S., Dissanayake, G., Nebot, E.M., Durrant-Whyte, H., ?An Experiment in Autonomous Navigation of an Underground Mining Vehcile,? IEEE Transactions on Robotics and Autonomation, 40(6), 1999, pp 85-95. 33. St. Pierre, M., Gingras, D., ?Comparison Between the Unscented Kalman Filter and the Extended Kalman Filter for the Position Estimation Module of an Integrated Navigation Information System,? IEEE Symposium on Intelligent Vehicles 2004, June 2004. 34. Stengal, R., ?Optimal Control and Estimation,? Dover Publications, Mineola, NY, ISBN: 0-486-68200-5, 1994. 35. Talaya, J., Alamus, R., Bosch, E., Serra, A., Kornus, W., Baron, A., ?Integration of a Terrestrial Laser Scanner with GPS/IMU Orientation Sensors,? International Society for Photogrammetry and Remote Sensing, XXXV, July 2004, pp 19-24. 36. Travis, W., Whitehead, R., Bevly, D.M., Flowers, G.T., ?Using Scaled Vehicles to Investigate the Influence of Various Properties on Rollover Propensity,? Proceedings of the 2004 American Controls Conference, June 2004, pp 3381-3386. 113 37. Travis, W., Bevly, D.M., ?Corridor Navigation with a Lidar/INS Kalman Filter Solution,? Proceedings of the IEEE Intelligent Vehicles Conference, June 2005, pp 343-348. 38. Travis, W., Bevly, D.M., ?Navigation Errors Introduced By Ground Vehicle Dynamics,? Proceedings of the ION GNSS 2005, September 2005. 39. Venhovens, P., Naab, K., ?Vehicle Dynamics Estimation Using Kalman Filters,? Vehicle System Dynamics, 32, 1999, pp 171-184. 40. Whitehead, R., Travis, W., Bevly, D.M., Flowers, G., ?A Study of the Effect of Various Vehicle Properties on Rollover Propensity,? SAE Paper 2004-01-2094, 2004. 41. Ye, C., Borenstein, J., ?Characterization of a 2-D Laser Scanner for Mobile Robot Obstacle Negotiation,? Proceedings of the 2002 IEEE International Conference on Robots and Automation, May 2002, pp 2512-2518. 114 APPENDICES APPENDIX A: DARPA GRAND CHALLENGE VEHICLE A small All Terrain Vehicle (ATV) platform, shown in Figure A.1, was chosen for the entry vehicle due to its size, agility, and ruggedness (Behringer, et al, 2004; 2005). The Prowler by ATV Corp. is a Yamaha Grizzly 660 modified for military use that is equipped with a 660cc Yamaha engine, enhanced suspension, full roll cage, run-flat tires, and cargo rack. This combination of power, ruggedness, and space proved to be an excellent foundation for an off road autonomous vehicle. The large payload capacity and heavy duty suspension can handle the multitude of motors, sensors, and computers that were mounted on the cargo rack and in the roll cage. The suspension and high ground clearance allow the vehicle to traverse difficult terrain with relative ease. Figure A.1: RASCAL at the starting gate during the 2005 DGC. 115 Modifications were made to the ATV, dubbed RASCAL (Robust Autonomous Sensor Controlled All terrain Land vehicle), for automation. A servo motor was installed in the engine bay and attached to the steering system to actuate the front wheels. The motor output, 6.5 ft-lbs of torque, was fed into a 14:1 gearbox, placing a total of 90 ft-lbs of torque on the steering rack. Throttle, brake, and gear were controlled with smaller servos. Figure A.2: Steering motor (left); throttle and brake servos (right). All of the servos were directed by microcontrollers, which communicated with a computer via serial ports. An emergency stop mechanism with ultimate authority over the microcontrollers was wired in series to both the vehicle?s power and the brake and throttle servos to provide the ability to stop and kill the vehicle in the event of a software or hardware failure. Two 15 gallon gas tanks were added to the side of the ATV and gave RASCAL more than enough fuel to finish the course. Two AC generators provided additional power needed to operate the electronics on board. An enclosure mounted in the rear contained the delicate hardware. 116 The autonomous system was designed to have a modular architecture, depicted in Figure A.3, so a failure from one module would not impede the operation of other modules. At the core are the necessary modules required for autonomous operation: the vehicle control and the GPS/INS navigation module. The other modules are ?optional add-ons? required for entry into the Grand Challenge. They provide information about the environment and about objects in it, which may need to be avoided in case they are on the path trajectory. These modules are considered ?optional? because a disconnection or failure for whatever reason will not impede the vehicle?s operation completely. The core vehicle control will still continue to operate, using solely GPS and inertial input for computing the control output. Figure A.3 Flow of information on RASCAL. One of the key tasks for RASCAL was to remain in a pre-defined corridor while choosing the fastest and/or easiest path through the corridor and successfully avoiding any obstacles within the corridor. Obstacle detection was realized with four Lidars and a stereo vision system custom built by ARC seibersdorf (Kolger, et al, 2006). Two of the 117 LIDARs scanned vertically to detected negative obstacles such as holes or ditches. One Lidar was mounted so that the scan plane was parallel to the ground to detect long range obstacles, and one was mounted so that the scan plane intersected the ground approximately five meters in front of the vehicle to detect obstacles near the vehicle. Each sensor data set was processed by an algorithm specialized to that sensor/orientation and then passed off for planning. Figure A.4: Obstacle detection sensors on RASCAL. Initially, most of the core technical members on this team were engineers at Rockwell Scientific (RSC), and the team received a large portion of its funding from RSC. However, in order to complement the existing expertise, SciAutonics sought other participants to join the team. A very significant partner was Auburn University, who 118 119 joined the SciAutonics team in November 2003, contributing to the vehicle control and navigation aspects of the system. ATV Corporation contributed by donating the vehicle platform and a second test vehicle, as well as providing continual technical support throughout the project. APPENDIX B: TEST VEHICLE B.1. TEST BED Infiniti G35 sedan was used as a test vehicle to gather experimental data for the research in this thesis. The car was loaned to the GPS and Vehicle Dynamics Lab at Auburn University. It is equipped with a data logging system that reads a multitude of sensors, including the vehicle?s onboard sensors over the Control Area Network (CAN). Figure B.1: Infiniti G35 with data logging console. A Navcom Starfire? DGPS receiver was utilized for its sub 10 centimeter position accuracy measurements at 5 Hz, and was used as a truth measurement when analyzing the algorithms with experimental data. A dual antenna Novatel Beeline? GPS receiver was used to measure vehicle heading and roll at 5 Hz, as well as provide standard, non- differential GPS measurements of velocity, course, and position. The GPS antennas are 120 shown mounted on the roof of the G35 in Figure B.2 on the right. A Crossbow IMU400CC automotive grade MEMS IMU measured accelerations and angular rates at 32 Hz. The IMU is shown mounted in the center console in the left figure below. The hand wheel angle was taken from the CAN and translated to a steering angle at the tire using a 16.1:1 steering ratio, also at 32 Hz. Figure B.2: The Crossbow IMU (left); Starfire and Beeline (right). Figure B.3 shows the data logging computer with the power distribution hardware, all mounted in the trunk. All power was provided by the vehicle and cleaned using DC-DC converters with 12VDC and 5VDC outputs. A 12VDC-ATX power supply was installed in the computer, which ran Windows XP. Communication to and from sensors was carried out with RS232 serial ports and a DAQ (data acquisition) card. 121 Figure B.3: The G35 data logging computer and power distribution center. The sedan was driven on a 1.7 mile oval test track with 8 degree banked turns, which was operated by Auburn University?s National Center for Asphalt Technology (NCAT). The straight sections are crowned 2 degrees in each lane. Experiments consisted of various maneuvers, including emergency lane changes, slaloms, and high speed cornering. The onboard vehicle dynamic control (VDC) was turned off, and the maneuvers were performed at speeds ranging from 5 to 45 m/s. B.2. SIDESLIP CALCULATION The heading and course measurements were used to calculate vehicle sideslip. Most receivers use a carrier phase differencing technique to calculate velocity and course, which injects an inherent half sample lag in the calculation. It is critical to synchronize all GPS measurements with respect to time, as any offset will calculate a false slip. The algorithm to compute slip was written so it could be implemented in real time. The heading and course measurements are output on two separate GPS messages. The heading measurement is time stamped with time into the week in GPS time. Course 122 and velocity measurements are time stamped with UTC time. GPS time was converted into time of day, and then converted to UTC time by accounting for the number of leap seconds since January 6, 1980. At the date of this thesis, the difference between GPS and UTC time was 14 seconds. The velocity and course measurements were then shifted by a half sample to remove the delay induced by computation. A linear interpolation between two consecutive measurements computes a value at time t that corresponds with the position based heading measurement recorded at time t. Sideslip can then be calculated by taking the difference between the two measurements. Equation B.1 shows how sideslip was used to convert the velocity vector in to lateral and longitudinal velocity at the antenna. )sin( )cos( _ _ BLBLBLy BLBLBLx VV VV ? ? = = B.1 The vehicle yaw rate was used in Equation B.2 to translate the velocities at the antenna to the CG. BLCGBLCG RrVV / ?+= B.2 123 124 APPENDIX C: VEHICLE SIMULATION PARAMETERS m = 1528.15 kg W = 149991.18 N I x = 535 kg?m 2 I y = 2222 kg?m 2 I z = 2400 kg?m 2 L = 2.8499 m a = 1.3679 m b = 1.4819 m h cg = 0.6 m t f = 1.5011 m t r = 1.5062 m s f = 1.2009 m s r = 1.2049 m K p = 28165.70 N?m/rad b p = 7850 N?m?s/rad K r = 17985.76 N?m/rad b r = 3850 N?m?s/rad C ?FL = 45837 N/rad C ?FR = 45837 N/rad C ?BL = 76394 N/rad C ?BL = 76394 N/rad ? f = 0.84 ? r = 0.81