Terrain Characterization and Roughness Estimation for Simulation and Control of Unmanned Ground Vehicles by Jeremy James Dawkins A dissertation submitted to the Graduate Faculty of Auburn University in partial fulfillment of the requirements for the Degree of Doctor of Philosophy Auburn, Alabama December 12, 2011 Keywords: Unmanned Ground Vehicles, Terrain Modeling, Terrain Characterization, Terrain Vehicle Interaction, Terrain Measurement, Fractal Surfaces Copyright 2011 by Jeremy James Dawkins Approved by David M. Bevly, Co-chair, Professor of Mechanical Engineering Robert L. Jackson, Co-Chair, Associate Professor of Mechanical Engineering Subhash C. Sinha, Professor of Mechanical Engineering Dan B. Marghitu, Professor of Mechanical Engineering ii Abstract This dissertation presents a methodology for generating artificial terrains for simulation of off-road vehicles. Furthermore it develops and evaluates methods for characterizing the terrain for the control of unmanned ground vehicles. The terrain is the principle source of chassis excitation in off-road vehicles and the control of the vehicle is dependent on effectively characterizing the terrain slope, roughness, and surface condition. The previous work in this area is presented and the areas for improvement are identified. The literature is vast and is categorized into works which have addressed various parts of the problems. It is advantageous for the development of autonomous vehicle systems to simulate the vehicle response over various terrains; this requires generating artificial terrains which are similar to real terrains. Two methods for generating terrains based on the Weierstrass-Mandelbrot (W-M) fractal function are presented. The generated surfaces are evaluated using the root mean squared elevation (RMSE) and power spectral density (PSD). A seven degree of freedom (7-DOF) suspension model is developed for the purpose of evaluating the response of the vehicle on the generated terrains. The vehicle response is used to introduce motion based metrics for characterizing the roughness of the terrain. The root mean squared (RMS) vertical acceleration, RMS roll rate, and RMS pitch rate are introduced as potential motion base metrics. Additionally the phase plane of various vehicle states is investigated as a means for understanding the vehicle state combined with the terrain roughness. iii A system for generating three dimensional point cloud maps of terrains is presented. Using a loosely coupled architecture Global Positioning System (GPS) and inertial navigation system (INS) are blended to provide estimates of the vehicle state. The system is implemented on the experimental vehicle to map various terrains. The terrain maps are characterized using RMSE, PSD, root mean squared slope (RMSS), and amplitude to wavelength ratio. Additionally, a feature extraction algorithm based on the wavelet transform is introduced. The response of the experimental vehicle on the terrains is analyzed using the RMS vertical acceleration, RMS roll rate, RMS pitch rate, and RMS suspension deflections. The 7-DOF suspension model of the experimental vehicle is then used to compare the simulated vehicle response to the experimental vehicle response. The model is then used to evaluate the effectiveness of the W-M function for generating artificial terrains. The response of the simulated vehicle on the experimental and generated terrains is then compared. It is determined that an artificial surface can be generated which will result in a similar vehicle response as the experimentally measured surface. The method does however have difficulty capturing the nuances of experimentally measured terrains. Additionally it is shown that the roughness of the terrain can be characterized by analyzing the surface with the RMSE, PSD, RMSS, or wavelength to amplitude ratio. The roughness can also be characterized by the RMS vertical acceleration, RMS roll rate, RMS pitch rate, or RMS suspension deflection. These methods are compared and their strengths and weaknesses are highlighted. iv Acknowledgments First, I would like to give glory and honor to my Lord and Savior Jesus Christ for the ability to complete this dissertation. I understand that I was blessed with an opportunity not many people have and I am thankful for all the experiences both good and bad. I would also like to thank my wife Shanee and the rest of family for their love and support throughout my graduate studies. I certainly would not have finished my without their prayers and guidance during this process. I would also like to thank my academic advisors Dr. David Bevly, and Dr. Robert Jackson. Their mentorship during the past four years was paramount in making me the engineer I am today. To my colleagues in the GPS & Vehicle Dynamics Lab and the Multi-Scale Tribology Lab, thanks for the support and the life-long friendships I have developed. Lastly, thank you to the Auburn University Office of Diversity and Multicultural Affairs for the additional support through the President?s Graduate Opportunity Program Fellowship. v Table of Contents Abstract ........................................................................................................................................... ii Acknowledgments.......................................................................................................................... iv List of Tables ............................................................................................................................... viii List of Figures ................................................................................................................................ ix Chapter 1: Introduction ............................................................................................................... 1 1.1 Motivation ........................................................................................................................ 1 1.2 Contributions .................................................................................................................... 3 1.3 Dissertation Organization ................................................................................................. 4 Chapter 2: Literature Review...................................................................................................... 6 2.1 Terrain Characterization ................................................................................................... 6 2.2 Terrain Modeling and Simulation .................................................................................. 10 2.3 Terrain Mapping and Terrain Measurement Systems .................................................... 11 Chapter 3: Terrain Modeling and Vehicle Simulation ............................................................. 13 3.1 Two Dimensional Terrain Profile Generation ................................................................ 13 3.2 Three Dimensional Terrain Surface Generation ............................................................ 17 3.2.1. 3-D Surface RMS Analysis ..................................................................................... 19 3.2.2. Power Spectral Density Analysis ............................................................................ 22 vi 3.2.3. Statistical Tests for Terrain Profiles ....................................................................... 24 3.3 Vehicle Simulation ......................................................................................................... 31 3.3.1. Vehicle Suspension Models .................................................................................... 31 3.3.2. Simulation Comparison .......................................................................................... 40 3.3.3. Vehicle Motion Based Metrics ............................................................................... 50 Chapter 4: Terrain Measurement and Statistical Analysis ....................................................... 56 4.1 Terrain Measurement System......................................................................................... 56 4.1.1. Experimental Hardware and Instrumentation ......................................................... 57 4.1.2. GPS/INS Loosely Couple Extended Kalman Filter ................................................ 60 4.1.3. LiDAR Terrain Mapping ........................................................................................ 71 4.2 Terrain Surface Analysis ................................................................................................ 76 4.2.1. Root Mean Squared................................................................................................. 81 4.2.2. Power Spectral Density ........................................................................................... 87 4.2.3. Amplitude to Wavelength ....................................................................................... 93 4.2.4. Wavelet Based Feature Extraction .......................................................................... 95 Chapter 5: Vehicle Experimental Response ........................................................................... 104 5.1 Analysis of Vehicle Motion Metrics ............................................................................ 104 5.2 Comparison of Simulated and Experimental Response ............................................... 115 5.3 Experimentally Based Terrain Generation ................................................................... 123 Chapter 6: Conclusions and Future Work .............................................................................. 135 vii 6.1 Terrain Roughness Summary ....................................................................................... 135 6.2 Terrain Modeling.......................................................................................................... 136 6.3 Terrain Mapping ........................................................................................................... 137 6.4 Future Work ................................................................................................................. 138 6.4.1. Real-time Analysis ................................................................................................ 138 6.4.2. Terrain Based Navigation and Control ................................................................. 139 Bibliography ............................................................................................................................... 141 Appendix A: Model Based Terrain Input Estimation ................................................................. 146 Appendix B: Experimental Test Site Description ....................................................................... 152 Appendix C: Experimental Hardware and Software Specifications ........................................... 160 C.1 Experimental Vehicle Specifications .......................................................................... 160 C.2 Sensor Specifications ................................................................................................... 162 C.3 Computer System ......................................................................................................... 167 viii List of Tables Table 3.1? PSD exponential fit constants for profiles taken from surfaces generated with various values of fractal dimension and scaling parameter . .................................... 24 Table 4.1 ? Summary of errors in GPS/INS solution ................................................................... 70 Table 4.2 ? Parameters fit to PSD for the measured longitudinal profiles ................................... 92 Table 4.3 ? Average and maximum amplitude ratios for the various terrain profiles measured. . 95 Table 5.1 ? RMS values for IMU signals for various surfaces ................................................... 110 Table 5.2 ? Area of least squares fit ellipse ................................................................................ 112 Table 5.3 ? Total RMS suspension deflections for various terrains ........................................... 115 Table 5.4 ? Parameters used in 7-DOF model of experimental vehicle ..................................... 117 Table 5.5 ? Standard deviations of measured states for 7-DOF model and experimental data .. 122 Table 5.6 ? Summary of RMS values for 7-DOF model simulated on longitudinal terrain profiles from dirt off-road ..................................................................................................... 132 Table 5.7 ? Summary of RMS values for 7-DOF model simulated on longitudinal terrain profiles from loose gravel ..................................................................................................... 133 Table 6.1 ? Surface and Motion based metrics for each of the terrains measured ..................... 135 Table C.1 ? Septentrio attitude errors assuming a 1 Hz update rate ........................................... 165 ix List of Figures Figure 3.1 ? Randomly generated 2-D terrain profiles with varying scaling parameters for the same fractal dimension ......................................................................................... 16 Figure 3.2 - PSD of generated fractal surface compared to ideal PSD fractal line. ...................... 16 Figure 3.3 ? Examples of surfaces generate using Weierstrass-Mandelbrot function with various values of D & G. ...................................................................................................... 19 Figure 3.4 ? Exponential relationship between RMS Elevation and Scaling parameter, for various fractal dimensions D. ............................................................................................... 21 Figure 3.5 ? RMS of left, middle, and right 2-D longitudinal profiles taken from 3-D surface compared with analytical fit for various fractal dimensions and scaling parameters. .................................................................................................................................. 21 Figure 3.6 ? Example of a PSD of a longitudinal profile taken from a 3-D fractal surface ......... 23 Figure 3.7 ? PSD fit exponential fit lines for a profile taken from surfaces generated with fractal dimension for various values of scaling parameter . ............................ 24 Figure 3.8 ? Linear regression residuals for a profile taken from surfaces generated with fractal dimension and (top) (middle) (bottom) .................................................................................................................. 26 Figure 3.9 ? Empirical distribution function compared with analytical Gaussian probability density function of profiles taken from surface with fractal dimension and (a) (b) (c) . ..................................................... 28 Figure 3.10 ? Windowed standard deviation over the length of a profile taken from a surface generated with and (top) , (middle) , (bottom) . ............................................................................................................ 30 Figure 3.11 ? Allan Deviation for profile taken from generated surface with fractal dimension and various values of scaling parameter plotted on linear scale. ....... 30 Figure 3.12 ? Allan Deviation for profile taken from generated surface with fractal dimension and various values of scaling parameter plotted on log-log scale. ..... 31 Figure 3.13 ? 2 Degree of freedom quarter car model. ................................................................. 32 x Figure 3.14 ? 4 Degree of freedom half car model. ...................................................................... 33 Figure 3.15 ? 7 Degree of Freedom Full Suspension Model. ....................................................... 34 Figure 3.16 ? Gain magnitude vs. frequency from wheel inputs to vehicle heave . ............. 36 Figure 3.17 ? Gain magnitude vs. frequency from wheel inputs to heave rate . .................. 37 Figure 3.18 ? Gain magnitude vs. frequency from wheel input to roll rate . .................... 37 Figure 3.19 ? Gain magnitude vs. frequency from wheel input to pitch rate . .................. 38 Figure 3.20 ? Gain magnitude vs. frequency from wheel input to suspension deflection . .............................................................................................. 38 Figure 3.21 ? Roll rate gain vs. terrain wavelength for various vehicle speeds ........................... 40 Figure 3.22 ? Heave motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error. .................................................................................................................. 44 Figure 3.23 ? Roll motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error. .................................................................................................................. 46 Figure 3.24 ? Pitch motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error ................................................................................................................... 47 Figure 3.25 ? Front un-sprung mass motion motion (a) comparison (b) error between 7-DOF model and CarSim .................................................................................................. 48 Figure 3.26 ? Rear un-sprung mass motion (a) comparison (b) error between 7-DOF model and CarSim. ................................................................................................................... 49 Figure 3.27 ? RMS vertical acceleration vs. speed for vehicle simulated on surfaces with varying scaling parameter G ................................................................................................ 51 Figure 3.28 ? RMS roll rate vs. speed for vehicle simulated on surfaces with varying scaling parameter G ............................................................................................................ 51 Figure 3.29 ? RMS pitch rate vs. speed for vehicle simulated on surfaces with varying scaling parameter G ............................................................................................................ 52 Figure 3.30 ? Kinetic Energy vs. speed for vehicle simulated on surfaces with varying scaling parameter G ............................................................................................................ 53 Figure 3.31 ? Phase plane plots of pitch rate vs. roll rate of vehicle simulated on terrains of various roughness at 10 MPH. ............................................................................... 54 Figure 3.32 ? Phase plane plots of roll rate vs. roll angle for simulated vehicle simulated on terrains of various roughness at 10 MPH. .............................................................. 55 xi Figure 3.33 ? Phase plane of pitch rate vs. pitch angle vehicle simulated on terrains of various roughness at 10 MPH. ............................................................................................ 55 Figure 4.1 ? Prowler ATV Experimental Test Bed ...................................................................... 58 Figure 4.2 ? Primary sensors used in mapping system (left) Novatel Propak v3 GPS Receiver (middle) Sick LMS 291 LiDAR (right) Crossbow 440 Inertial Measurement Unit 58 Figure 4.3 ? Mounting of LiDAR on front of Prowler ATV ........................................................ 59 Figure 4.4 ? Path driven during survey comparison of GPS data and GPS/INS .......................... 66 Figure 4.5 ? Comparison of positions using RTK GPS (blue) and GPS/INS (red) ...................... 67 Figure 4.6 ? Vehicle velocities comparing GPS (blue) to GPS/INS (red) .................................... 68 Figure 4.7 ? Attitude measurements for (a) comparison of GPS/INS estimate and Septentrio (b) errors between GPS/INS and Septentrio .................................................................. 69 Figure 4.8 ? Block diagram for sensor fusion of GPS, IMU, and LiDAR for terrain mapping system ...................................................................................................................... 72 Figure 4.9 ? Point cloud map of parking lot in east north up coordinate frame ........................... 73 Figure 4.10 ? Point cloud map of dirt path leading to open rough asphalt area in (a) global path coordinates (b) path coordinates relative to the vehicle (flattened) ....................... 75 Figure 4.11 ? Point cloud map of high grassy terrain in path coordinate frame relative to the vehicle .................................................................................................................... 76 Figure 4.12 ? Longitudinal terrain profiles taken from smooth asphalt terrain map .................... 78 Figure 4.13 ? Longitudinal terrain profiles taken from packed gravel ......................................... 79 Figure 4.14 ? Longitudinal terrain profiles taken from rough asphalt .......................................... 79 Figure 4.15 ? Longitudinal terrain profiles taken from loose gravel ............................................ 80 Figure 4.16 ? Longitudinal terrain profiles taken from dirt off road path .................................... 80 Figure 4.17 ? Longitudinal terrain profiles taken from dirt path transitioning to rough asphalt .. 81 Figure 4.18 ? RMS elevation of 2-D longitudinal road profiles from smooth asphalt ................. 82 Figure 4.19 ? RMS elevation of 2-D longitudinal road profiles from packed gravel ................... 83 Figure 4.20 ? RMS elevation of 2-D longitudinal road profiles from rough asphalt ................... 83 Figure 4.21 ? RMS elevation of 2-D longitudinal road profiles from loose gravel ...................... 84 xii Figure 4.22 ? RMS elevation of 2-D longitudinal road profiles from dirt off-road ..................... 84 Figure 4.23 ? RMS elevation of 2-D longitudinal road profiles from dirt path transitioning to rough asphalt .......................................................................................................... 85 Figure 4.24 ? RMS slope of 2-D longitudinal road profiles for smooth asphalt .......................... 86 Figure 4.25 ? RMS slope of 2-D longitudinal road profiles for packed gravel ............................ 87 Figure 4.26 ? PSD of longitudinal profiles taken from smooth asphalt map shown with mean curve fit ................................................................................................................... 89 Figure 4.27 ? PSD of longitudinal profiles taken from packed gravel map shown with mean curve fit ................................................................................................................... 89 Figure 4.28 ? PSD of longitudinal profiles taken from rough asphalt map shown with mean curve fit ............................................................................................................................ 90 Figure 4.29 ? PSD of longitudinal profiles taken from loose gravel map shown with mean curve fit ............................................................................................................................ 90 Figure 4.30 ? PSD of longitudinal profiles taken from off-road dirt map shown with mean curve ................................................................................................................................ 91 Figure 4.31 ? PSD of longitudinal profiles taken from dirt path transitioning to rough asphalt map shown with mean curve fit ............................................................................. 91 Figure 4.32 ? Amplitude to wavelength ratio for smooth asphalt surface .................................... 94 Figure 4.33 ? Amplitude to wavelength ratio for off-road dirt ..................................................... 94 Figure 4.34 ? Point cloud of smooth asphalt with objects scattered along vehicle path .............. 96 Figure 4.35 ? Examples of various mother wavelet functions ...................................................... 97 Figure 4.36 ? Smooth asphalt terrain loops showing (a) points of interest (b) extracted features. ................................................................................................................................. 100 Figure 4.37 ? Packed gravel terrain loops showing (a) points of interest (b) extracted features. ................................................................................................................................. 101 Figure 4.38 ? Rough asphalt terrain loops showing (a) points of interest (b) extracted features. ................................................................................................................................. 102 Figure 5.1 ? Windowed RMS vertical acceleration, roll rate, and pitch rate for vehicle driving on packed asphalt (a) raw signal (b) velocity compensated signal ............................. 106 Figure 5.2 ? Compensated RMS vertical acceleration, roll rate, and pitch rate for the smooth asphalt .................................................................................................................... 107 xiii Figure 5.3 ? Compensated RMS vertical acceleration, roll rate, and pitch rate rough asphalt ... 108 Figure 5.4 ? Compensated RMS vertical acceleration, roll rate, and pitch rate for loose gravel 108 Figure 5.5 ? Compensated RMS vertical acceleration roll rate and pitch rate for dirt off-road . 109 Figure 5.6 ? Compensated RMS vertical acceleration roll rate and pitch rate grassy off-road .. 109 Figure 5.7 ? Phase plane plot of pitch rate and roll rate with least squares fit ellipse of data plotted for off-road dirt terrain. .............................................................................. 111 Figure 5.8 ? Phase plane plot of pitch rate and roll rate with least squares fit ellipse of data plotted for vehicle performing a sine steer maneuver on smooth pavement. ........ 113 Figure 5.9 ? Suspension deflections of the front suspension for (a) smooth asphalt (b) dirt off- road ........................................................................................................................ 115 Figure 5.10 ? Diagram of suspension geometry of experimental vehicle .................................. 118 Figure 5.11 ? Heave motion comparison of experiment (blue) and model (red) ....................... 120 Figure 5.12 ? Roll motion comparison of experiment (blue) and model (red) ........................... 120 Figure 5.13 ? Pitch motion comparison of experimental (blue) and model (red) ....................... 121 Figure 5.14 ? Suspension deflection comparison of experimental (blue) and model (red) ........ 121 Figure 5.15 ? Comparison of experimental loose gravel terrain profile (top) with regenerated partial fractal terrain profile (bottom) .................................................................. 125 Figure 5.16 ? Dirt off-road longitudinal terrain profiles from terrain map (top) compared to longitudinal terrain profiles from randomly generated 3-D surface using Weierstrass-Mandelbrot function with parameters determined from fractal parameter RMSE relationship (bottom). .............................................................. 127 Figure 5.17 ? Loose gravel longitudinal terrain profiles from terrain map (top) compared to longitudinal terrain profiles from randomly generated 3-D surface using Weierstrass-Mandelbrot function with parameters determined from fractal parameter RMSE relationship (bottom). .............................................................. 127 Figure 5.18 ? Power spectral densities for loose gravel experimental surface and regenerated fractal surface ....................................................................................................... 128 Figure 5.19 ? Power spectral densities for loose gravel experimental surface and regenerated fractal surface ....................................................................................................... 129 Figure 5.20 ? Heave motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface ................................................................. 130 xiv Figure 5.21 ? Roll motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface .................................................................................... 131 Figure 5.22 ? Pitch motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface ................................................................. 131 Figure 6.1 ? Block diagram showing potential architecture for terrain based vehicle controller140 Figure A.1 ? Estimation of road profile using 7DOF model based two-step Kalman filter (a) Left profile (b) Right profile .......................................................................................... 150 Figure B.1 ? Smooth Asphalt terrain from NCAT parking lot ................................................... 153 Figure B.2 ? Rough Asphalt terrain ............................................................................................ 153 Figure B.3 ? Rough Asphalt terrain ............................................................................................ 154 Figure B.4 ? Packed Gravel terrain............................................................................................. 155 Figure B.5 ? Packed Gravel terrain............................................................................................. 155 Figure B.6 ? Packed Gravel terrain............................................................................................. 156 Figure B.7 ? Loose Gravel terrain .............................................................................................. 157 Figure B.8 ? Loose Gravel terrain .............................................................................................. 157 Figure B.9 ? Dirt Off-Road terrain ............................................................................................. 158 Figure B.10 ? Dirt Off-Road terrain ........................................................................................... 158 Figure B.11 ? Transition from Dirt Path to Rough Asphalt ....................................................... 159 Figure B.12 ? Transition from Dirt Path to Rough Asphalt ....................................................... 159 Figure C.1 ? Sick LMS 291 LiDAR ........................................................................................... 162 Figure C.2 ? Crossbow 440 Inertial Measurement Unit ............................................................. 163 Figure C.3 ? (left) Novatel GPS antenna (right) Novatel ProPak v3 receiver ............................ 164 Figure C.4 ? Sepentrio PolaRx2 multi-antenna GPS receiver .................................................... 165 Figure C.5 ? (left) Celesco MLP-125 linear potentiometer (right) mounting of potentiometer parallel to suspension coilovers. ............................................................................ 166 Figure C.6 ? Advantech rugged PC ............................................................................................ 167 1 Chapter 1: Introduction 1.1 Motivation There is a strong push in the Army to move towards autonomous and semi-autonomous vehicles to perform tasks which may be too cumbersome or dangerous for human driven vehicles to perform. Unmanned ground vehicles (UGVs) can potentially be used to find and disarm improvised explosive devices (IEDs) without risking the lives of soldiers. Autonomous vehicles in convoys can free soldiers to focus on other tasks such as identifying potential threats. Additionally autonomous vehicles are far superior to humans at performing repetitive tasks without fatigue. The development of UGV systems requires a thorough understanding of interaction between the vehicle and the terrain on which it is operating. The terrain is the principle input into the suspension and causes the vehicle to roll and pitch. In a manned vehicle, the driver makes decisions on how to control the vehicle based on the roughness of the terrain. For example if the terrain is too bumpy the driver will slow up in order to minimize the loads on the vehicle. Another advantage a human driver has is the ability to determine the best path to take based on the perceived terrain. Mimicking the human driver?s decision making abilities on an unmanned vehicle system is a challenging task. Many of the metrics natural for humans to use in describing the roughness of a terrain are qualitative. One driver may describe a certain terrain as bumpy or another terrain as rocky. These descriptions may change for a different driver or the same driver in a different vehicle. The very nature of the terrain characterization process is situation dependent and highly subjective. In order to effectively control the vehicle, one must determine 2 methods of describing the terrain quantitatively. In this way one terrain can be compared to another, and the unmanned vehicle can make decisions based on a metric it can understand. This allows the vehicle control laws to be altered to account for the dynamics which are caused by the terrain. Designing a controller to effectively control a UGV over off-road terrains is a non-trivial task. The appropriate course of action for the vehicle is not always the intuitive solution. Under certain situations, such as resonance, the vehicle can speed to reduce the vehicle oscillations. Of course speeding up can improve the efficiency of the mission completion, but increasing the speed also comes with other issues such as increased roll over propensity. Thus, it becomes important to conduct experiments and simulations on off-road terrains to better understand how the vehicle will behave under varying conditions. Unfortunately, experimental testing of off road vehicle dynamic maneuvers can prove to be problematic for various reasons. It is difficult to find testing locations which accurately represent the terrain on which the vehicle will be required to operate. Additionally, off-road vehicle dynamic experiments can be more dangerous than their on-road counterparts. Developing these methodologies in simulation allows one to cover a wider range of scenarios while avoiding the danger and expense of running numerous vehicle tests. Currently, advanced UGV simulation environments are being developed in industry and being used for analysis of robotic systems [1]. One such system is the Robotics Interactive Visualization and Experimental Toolbox (RIVET) which was developed under the Army Research Laboratory?s Robotics Collaborative Technology Alliance. These environments allow algorithms to be tested in a safe and efficient manner. However, as the vehicle path planning and control algorithms become more sophisticated, they require the simulated vehicle to behave more 3 like the real vehicle. Occasionally it is observed that the real vehicle exhibits behaviors that are not seen in simulation. Capturing these behaviors in simulation requires effectively modeling the vehicle?s interactions with its environment. Typically more advanced UGV?s include various vision sensors such as cameras, and LiDARs. Many path planning and obstacle avoidance algorithms are based on the measurements from these sensors. In service, noise will be added to the measurements from these sensors based on the motion of the vehicle, caused by the vehicle?s interaction with the terrain. Thus, it is important to the development of the simulation environment to accurately model the terrain-vehicle interaction. 1.2 Contributions One of traits that make this dissertation unique from previous works in this area is the combination of simulation and experimental studies. Most works have focused either on experimental studies or developing methodologies for simulation of ground vehicles. It is beneficial to validate the simulation methods with experimental data to increase their application to the actual system. Additionally, here the problem of terrain characterization is approached from the perspective of vehicle controller design rather than simply understanding the terrain. There are several key contributions that this dissertation provides: ? An experimentally validated methodology for generating artificial terrains for the simulation of ground vehicles using the Weierstrass-Mandelbrot fractal function. It is also shown that the response of a vehicle simulated on the generated surfaces closely matches the response of the vehicle simulated on the actual terrains. ? A statistical analysis regarding the linearity, Gaussianity, and stationarity of the surfaces generated with the Weierstrass-Mandelbrot fractal function. 4 ? An experimentally validated 7-DOF suspension model is developed of the experimental vehicle for comparing measured terrains and fractal terrain models. ? A comparison of various surface based and motion based terrain roughness metrics based on experimentally measured terrains is provided. ? The problem of state estimation in the presence of unknown inputs for estimating the terrain profiles from measured vehicle states is discussed. 1.3 Dissertation Organization This section discusses the organization of the remainder of the dissertation. Chapter 2 presents the relevant literature to the problem this dissertation seeks to solve. The literature is vast and is categorized into works which have addressed various parts of the problems. Chapter 3 focuses on the modeling and simulation of ground vehicles on rough terrains. Methods for generating terrain models are presented and those methods are analyzed. The last section of chapter three introduces the vehicle model which is used to evaluate the terrain modeling methods. The response of the model simulated on the terrains is then evaluated. Chapter 4 presents the methods used to experimentally measure and evaluate various rough terrains. The experimental hardware is introduced and the methodology for mapping terrains is presented. The maps are then analyzed using various methodologies. Chapter 5 is primarily focused on understanding the vehicle response over the experimentally measured terrains. The terrains are further evaluated using metrics based on the motion of the vehicle. The last section of the chapter compares the measured terrains to artificially generated terrains using a vehicle model. Chapter 6 summarizes the conclusions which were determined from the previous chapters. Additionally, the application of the methods to a vehicle controller is framed. Appendix A introduces a model 5 based estimator which can be used to estimate the terrain profile using a vehicle model. Appendix B presents photographs and descriptions of locations where data was collected. 6 Chapter 2: Literature Review Presented in this chapter is summary of the relevant work which has been conducted in areas related to the problem of terrain characterization and roughness estimation for ground vehicles. Section 2.1 describes the work which has addressed the broad issue of terrain characterization. The work which investigates modeling and simulation of terrain profiles is presented in Section 2.2. Section 2.3 introduces the terrain mapping problem and the associated literature. 2.1 Terrain Characterization The study of terrain vehicle interaction has been a topic which has been researched for quite some time [2,3]. There is a vast amount of literature addressing various aspects of modeling the interface between the tire (or track) and the terrain. The term terrain characterization has been used rather loosely in the literature. Researchers have used it to refer to understanding the mechanical properties of the terrain called terramechanics. Determining how the terrain properties affect a vehicle?s ability to traverse it, which has also been referred to as trafficability. It has also been used to describe the process of classifying the type of terrain (i.e. sand, dirt, gravel). One will also see the term terrain characterization used for larger scales than relevant for vehicle dynamic studies, such as in geological surveys or aerial vehicle mapping. Many of these works use digital elevation models (DEM) to represent the terrain. The focus of this section will be to present the literature as it relates to the development and testing of ground vehicle systems. Thus, the literature presented here will include terrain characterization as it relates to mechanical properties, small ground robots, and larger ground vehicles. 7 Terramechanics is an incredibly broad and well developed field. Researchers are largely concerned with understanding and modeling the mechanical behavior of the terrain surfaces. As one might expect this type of work has numerous applications, one of which is in the interaction of wheeled and tracked vehicles with the terrain surface. Experimental testing of various surface properties has occurred for well over 60 years. The work in this area has been led by various Army research labs. There are three primary tests which are used to analyze the properties of a surface [4]. The penetration resistance test is one which uses a device called a penetrometer. The penetrometer is pressed into the ground and the force required for a given depth of penetration is measured. Another test which is performed is called the plate sinkage test, which is used to determine the pressure-sinkage relationship. This is essentially testing the flotation characteristics of the soil. In this test, a plate approximately the size of the tire contact is pressed onto the soil to test the load the soil can support. The last test that is commonly used to measure the mechanical properties of soil is a shear strength test. Shear strength tests are important for the terrain vehicle interaction since they give information regarding the tractive properties of the soil. There are several devices which have been developed to measure the shear strength in various ways, such as the shear vane, the Cohorn sheargraph, and the annular shear ring. Additionally, a device called the bevameter can perform shear and plate tests simultaneously. The bevameter can be either mounted to a vehicle [5,6] or smaller handheld versions can be used [7]. Much of the work in this area is based on empirical data and it would be beneficial to develop more model based approaches. The NASA Jet Propulsion Lab has been directly or indirectly associated with much of the work done in the field of terrain classification for navigation of small ground robots, developing and sponsoring significant work in the field. Manduchi et al. developed a method for terrain 8 classification using a stereo vision system and single axis LiDAR mounted to a wheeled robot [8]. Using a color classification method they were able to develop an obstacle detection method which can operate successfully in areas with high vegetation. Iagnemma et al. has developed methods of classifying the terrain based on sinkage and wheel torque measurements [9,10,11,12,13]. Based on the mechanics of the wheel terrain contact, parameters relating to the deformation and stresses in the surface can be estimated [10,11]. The sinkage of the wheel in the soil was determined using a camera. It was also shown in this work that these parameters could be estimated on-line. The application for this work was in the development of technologies for the Mars rover. Ojeda et al. showed that the terrain can be classified based on the motor current required for the wheeled robot to perform certain simple maneuvers [14]. Using this method they were able to distinguish whether the robot was driving on gravel, dirt, or sand. Another work which has applied terrain characterization to small mobile robots, has implemented a terrain based velocity controller to a two wheeled Segway platform [15]. The robot was equipped with a LiDAR scanning the ground in front of the robot, and based on the transverse roughness of the ground in front of the vehicle the longitudinal velocity is adjusted. The metrics used to characterize the roughness used were based on various root mean squared (RMS) calculations for the transverse profile obtained. The behavior of larger ground vehicles is often fundamentally different from small ground robots. Many small ground robots are skid steer while large ground vehicles are Ackerman steer. Larger ground vehicles typically have suspensions and higher center of gravities, which require accounting for dynamics not present in small ground robots. Also generally speaking larger vehicles are more capable than small vehicles; terrains which small robots need to avoid a larger vehicle can cross. Thus, it is important to understand how the 9 terrain vehicle interaction will affect the dynamics of the vehicle. Various Army agencies are interested in this problem for vehicle testing, although the techniques developed can be applied to autonomous vehicle development and control. The Army has traditionally used root mean squared elevation (RMSE) as a means for terrain characterization [16,17,18]. RMSE is effective at providing a simple metric which is computationally efficient and indicative of the general roughness of the terrain. More recently, the RMSE has been paired with the power spectral density (PSD) to provide a more complex terrain characterization [19,20,21]. The PSD characterization takes into consideration the spatial frequency content of the terrain which allows it to capture trends in the terrain which cannot be captured by RMSE. Unfortunately both of these methods require some strong statistical assumptions in order to obtain an accurate characterization. The profiles must be statistically Stationary, Gaussian, and linear. Chaika et al. summarized the statistical tests for these traits of a terrain profile [22]. Unfortunately, off-road terrain profiles are typically non-stationary and non- Gaussian making these metrics not ideal for off-road profile characterization, yet they are still effective and frequently used a basis for comparison to other methods. Li and Sandu modeled stationary road profiles with auto-regressive moving average (ARMA) models [23]. They related this characterization to the values given by the International Roughness Index (IRI). Kern and Ferris showed that using the auto-regressive integrated moving average (ARIMA) is an effective way to characterize a non-stationary 2-D profile [24,25]. They applied these techniques for modeling general road profiles. Wagner and Ferris extended this method to reduce the order of the model required by using singular value decomposition [26]. One characterization method which has been widely adopted for use in characterizing the roughness of road profiles is the international roughness index (IRI). IRI is the standard for all 10 state departments of transportation. Based on the motion of a quarter car model simulated over a road profile, the IRI value is calculated [27,28,29]. This allows any profiling method to be used to determine the geometry of the profile. Howe et al. have developed a method called the speed roughness index (SRI) which modifies the IRI to incorporate speed into the metric [17]. 2.2 Terrain Modeling and Simulation It is important to conduct experiments and simulations on off-road terrains to better understand how the vehicle will behave under varying conditions. Unfortunately, experimental testing of off road vehicle dynamic maneuvers can prove to be problematic for various reasons. It is difficult to find testing locations which accurately represent the terrain on which the vehicle will be required to operate. Additionally, off-road vehicle dynamic experiments can be more dangerous than their on-road counterparts. Developing these methodologies in simulation allows one to cover a wider range of scenarios while avoiding the danger and expense of running numerous vehicle tests. Thus, it is desirable to develop methods for generating artificial terrains for the purpose of vehicle dynamic simulation. There are inconsistencies in the literature regarding the terminology for terrain modeling. Some researchers have referred to profiles and surfaces as 1-D and 2-D respectively, while other researchers have referred to them as 2-D and 3-D respectively. This work will refer to profiles as 2-D and surfaces as 3-D. There have been several works which have generated 2-D terrain profiles for the purpose of vehicle modeling. Li and Sandu have developed methods of modeling profiles using polynomial chaos methods [30] and using ARMA models [23], both of which are effective in generating profiles which can be used for simulation. A work by Lee and Sandu used stochastic partial differential equations to model profiles [31]. The ARIMA models used by Kern and Ferris can also be used to generate rough terrain profiles [25]. Howe et al. have generated 2-D 11 profiles using fractal Brownian motion, implemented with the midpoint displacement method [17]. The literature on methods for generating 3-D terrain surfaces for vehicle simulation is not as well developed as for 2-D profiles. However, some of the 2-D methods can be extended quite nicely to 3-D. Li and Sandu extended the ARMA model to create 3-D terrain surfaces [23]. They simulated quarter car suspension models on the terrains for validation with the IRI. Additionally Wang et al. developed methods of generating 3-D fractal terrains using Brownian motion for vehicle testing in simulation, but the implications on the vehicle simulation aspect was not thoroughly discussed [32]. There is a gap in the literature for a thorough discussion of the behavior of complex vehicle models on fully 3-D terrain surface. This dissertation seeks to provide a more in-depth analysis of a complex vehicle model on simulated and experimentally measured terrains. The response of the model is validated and compared against the experimental vehicle?s dynamics response under varying terrain conditions. 2.3 Terrain Mapping and Terrain Measurement Systems The literature on systems which use LiDAR and cameras for terrain mapping for robotics is vast. The combination of LiDAR and stereo camera has been used for small mobile robots and autonomous ground vehicles. LiDARs pointed in various directions around the vehicle can create 3-D point clouds of the environment surrounding the vehicle [33]. Most often the purpose of these types of systems is for obstacle detection, high speed navigation, or surveying [33,34,35]. There are a couple of works which have developed terrain measurement systems (TMS) specifically for 2-D/3-D profiling and characterizing the terrain roughness. The typical TMS uses LiDAR or other laser scanning device coupled with GPS and IMU blended in a Kalman filter. Two works [36,37] developed and experimentally tested a TMS which is based on this 12 general principal. The system which was mounted to an SUV provides high resolution 3-D mapping and appears to work well on road. However, no off-road data was presented to validate its performance in off-road scenarios. Dembski et al. developed a system specifically for measuring off-road terrain [38]. Using three laser scanners along the front of an ATV they were able to accurately measure 2-D road profiles along the longitudinal path of the vehicle. This dissertation develops a LiDAR based mapping system and demonstrates its performance in an off-road environment. There are also several practical issues which are encountered when performing LiDAR based terrain measurements in off-road scenarios. These issues are presented and their effect on the terrain roughness characterization is addressed. 13 Chapter 3: Terrain Modeling and Vehicle Simulation This chapter focuses on the development and validation of methods for generating terrains for vehicle simulation. Section 3.1 introduces a method for generating 2-D terrain profiles. The created 2-D profile can be used for simulation of simple vehicle suspension models such as a quarter car model, or a planar pitch vehicle model. Additionally they can be used for longitudinal vehicle simulations with full suspension models under the assumption that the left and right wheels travel over the same profile. Section 3.2 extends the method presented in Section 3.1 to 3-D terrain surfaces. 3-D surfaces can be used in more complex vehicle simulations of full suspension models, accounting for vehicle heave, pitch, and roll. These surfaces are analyzed using various terrain roughness characterization methods. Section 3.3 shows the results from vehicle simulations conducted on 3-D terrain surfaces and introduces vehicle motion based terrain metrics. 3.1 Two Dimensional Terrain Profile Generation For the purposes of developing and testing terrain characterization methods, it is beneficial to be able to generate random profiles that match terrains with varying degrees of roughness. This allows vehicle simulations to be run on various terrains with less empirical data necessary. A fractal surface is one which has the property of self-similarity, which is to say that smaller scales are a reduced size copy of the whole [39]. Although strictly speaking terrain profiles are not self-similar, fractals can be found in many places in nature and for this reason it has become a rather common method of modeling terrain profiles. There are several methods of generating a fractal profile two of which are midpoint displacement and fractional Brownian 14 motion. A terrain profile can be represented as self-similar fractal surface if the PSD of that profile exhibits a power law behavior with increasing frequency. There are several works which have modeled terrain profiles as fractals for the purposes of terrain characterization [17,18,40,32]. An ideal fractal surface will have a PSD of the following form (3.1) where R is the roughness amplitude constant, ? is the wave number (spatial frequency) in cycles/m, and k is the slope of the wave number spectrum. This form can be fit to the PSD of a terrain profile to determine the constants R and k. As with any fit, this will average out the effects of certain features which may be important from a vehicle dynamics standpoint. However, this does allow a profile which will capture the general trend of the terrain roughness to be generated. These parameters must be related to a function which can generate a fractal profile. A two-dimensional fractal surface profile , can be represented by a Weierstrass?Mandelbrot (W-M) function that satisfies the properties of continuity, nondifferentiabilty and self-affinity. The W-M function classifies a rough surface based on two fractal parameters. In this dissertation the following version has been used for dimensional consistency [41] ( ) ? ( ) (3.2) where and are the fractal roughness parameter and fractal dimension of the surface profile respectively. The parameter is an integer that represents a frequency level of the surface, is a parameter that determines the relative phase difference between fractal modes, and is a random phase shift for each frequency level of the surface. In this dissertation, and L is the length of the profile generated. is a uniformly distributed random value for each frequency level such that . 15 Additionally, the continuous power spectrum for a fractal surface is given by [42] ? (3.3) The fractal parameters and can be determined for a given and by setting Equations (3.1) and (3.3) equal to each other resulting in the following expressions (3.4) (3.5) After determining the fractal parameters a surface can be generated using Equation (3.2) . By randomly selecting the phase shift, the profiles generated will be significantly different even though they were generated from the same fractal parameters. Figure 3.1 shows two profiles which were generated using the W-M function with different fractal parameters, resulting in two surfaces with different roughness. To validate the generated fractal profile, the PSD extracted from the generated surface is plotted against the ideal PSD line used to fit the fractal parameters. It can be seen from Figure 3.2 that the PSD of the generated profile matches the trend of the ideal line. 16 Figure 3.1 ? Randomly generated 2-D terrain profiles with varying scaling parameters for the same fractal dimension . Figure 3.2 - PSD of generated fractal surface compared to ideal PSD fractal line. 17 3.2 Three Dimensional Terrain Surface Generation The 2-D Weierstrass-Mandelbrot function presented in Section 3.1, can be extended to 3- D to create fractal surfaces. The 3-D W-M function is given by Equation (3.6). Like the 2-D version the surface is built from stacking sinusoids of increasing frequencies up to a maximum frequency level. This is a multivariate function which is a function of the position in two perpendicular directions. The key difference between the 2-D and 3-D versions of the W-M function is the use of superimposed ridges upon which the surface is built. More details about the W-M function can be found in a paper by Ausloos and Berman [43]. ? ? * ( ) , * ( ) + -+ (3.6) where, ( ) ( ) (3.7) As in the 2-D version, the roughness of the surface is governed by the parameters and , the fractal dimension and scaling coefficients respectively. is a measure of the amplitude roughness which effectively scales the surface, and is the transverse width of the profile. The parameter controls the density of frequencies in the surface. M is the number of superimposed ridges used to reconstruct the surface profile and the parameter is a frequency index with being some upper cutoff frequency. are random phases among the sinusoids which create the surface. Since in this dissertation only terrains along a vehicle path are being considered, it is assumed the path is much longer than it is wide. Thus, the transverse width of the profile is used rather than the length so as not to improperly scale the surface. For a 3-D surface , and in general the surface will appear more flat when is close to 2 and become jagged as 18 approaches 3. Typically the values of these parameters will be in the middle of the range, not near the extremes. The overall roughness of the surface is defined by the combination of and . Additionally by randomly selecting the phase shift, unique surfaces can be created using the same fractal parameters. This can be beneficial for simulating a vehicle on multiple scenarios with the same statistical roughness. Note as the frequencies in the surface increase, the amplitude decreases. Thus, at a certain frequency level the changes in the surface will no longer affect the dynamics of the vehicle. Figure 3.3 shows four surfaces which were generated using this method, two surfaces with and two with . Each surface has a different G value. It can be seen that surfaces with a large range of roughness can be generated using this method. In general as is increased the surface will become rougher for a given . However, the particular magnitude of required to yield a desired roughness can change significantly based on the current . This relationship can be seen by examining Equation (3.7) which describes the overall scaling of the surface. When is closer to 2 the exponent is closer to zero making the quantity less sensitive to variations in . As approaches 3 the scaling factor becomes more dependent on . The effect of this on generating a desired terrain surface is that it requires the correct combination of and to be selected in order to yield the desired roughness. It should be noted that the roughness of the surfaces created using the W-M function are not strictly isotropic, although any variations in roughness along different directions are negligible from a vehicle dynamics standpoint. In general the fractal dimension can be determined from the power spectral density of an experimental surface. However, the accuracy of determined from this method will be dependent on the self-affinity of the surface. The self-affinity assumption breaks down for most experimentally measured terrains. This process is discussed further in Chapter 5. 19 Figure 3.3 ? Examples of surfaces generate using Weierstrass-Mandelbrot function with various values of D & G. 3.2.1. 3-D Surface RMS Analysis The root mean squared elevation (RMSE) is a common method of describing the roughness of any type of surface, thus it is a popular way to characterize terrain profiles as well. The RMSE can be calculated with the following expression, ? ? (3.8) where is the profile height in meters and is the number of samples. One of the drawbacks to this method is that in order for it to accurately represent the profile?s roughness the profile must be stationary. That is to say, the statistics do not change over the length of the profile. If one considers a profile for which the first half is smooth and the last half is rough, the RMSE for the entire profile will be an average of the two, which does not accurately represent the true profile. 20 Since the profiles being tested here are non-stationary the RMSE can vary for subsections of the profiles. The RMSE will be more accurate for shorter profile lengths. Figure 3.4 shows the RMSE as a function of scaling coefficient G for each of the terrain surfaces generated. The data points are the calculated RMSE values for each of the surfaces. Since there is an exponentially increasing trend in RMSE as G is increased, an exponential equation was fit to each set of data. It is interesting to observe that there are several combinations of G and D that will yield a surface with the same RMSE. This figure also shows how the range of G values can change based on D. It should be noted that the RMSE will vary based on the length of the profile and the parameter . The equations shown here were determined assuming the length of the profile is 100m and . Figure 3.5 shows the RMSE data points for the left, middle, and right profiles plotted on top of the fit curve. This gives an indication of the variation of the RMSE of profiles in different transverse locations of the surface. It can be seen that as the surface increases in roughness there is more variation in the RMSE of individual profiles taken from the surface. 21 Figure 3.4 ? Exponential relationship between RMS Elevation and Scaling parameter, for various fractal dimensions D. Figure 3.5 ? RMS of left, middle, and right 2-D longitudinal profiles taken from 3-D surface compared with analytical fit for various fractal dimensions and scaling parameters. 22 3.2.2. Power Spectral Density Analysis The power spectral density (PSD) has been shown to be an effective way to characterize off road terrain profiles [19,20,21]. The PSD of an off road terrain will have a decreasing trend with increasing wave number (spatial frequency). In this dissertation the Welch?s method was used to calculate the PSD, which can be implemented in Matlab using the command ?pwelch?. The PSD can also be calculated by taking the Fourier transform of the autocorrelation of the profile, with similar results. Figure 3.6 shows the PSD of a longitudinal profile taken from the fractal terrain surface. It is often beneficial to inspect the PSD to understand the frequency content in the profile. For example in Figure 3.6 it can be seen that for certain windows of wave numbers there is more energy present than in the general downward trend of the data. The dip in magnitude at the lower wavenumbers (less than 0.1) is caused by the fractal surface not including any road grade. As previously mentioned, it is common to fit a line to the PSD data which captures the trend of the data. The parameters that define the line are then used to classify the roughness of the terrain. Recall, the linear fit of a PSD will have the form (3.9) where is the PSD , is the wave number , and where and are constants which relate to the scaling and the slope of the fit line respectively. It should be noted that one of the drawbacks to this method is that taking a line fit on the PSD data can lose important information about a surface. As seen in Figure 3.6, a line fit cannot capture the existence of increased energy in higher frequency content of a profile, which can be important especially considering the effect of this frequency on the vehicle response. Additionally, like the RMSE the PSD inherently makes the assumption that the profile is statistically stationary. Thus for the profiles tested there could be variations in the PSD for shorter or longer profile lengths. 23 Figure 3.6 ? Example of a PSD of a longitudinal profile taken from a 3-D fractal surface Table 3.1 shows the PSD fit parameters for terrain surfaces which were generated using various values of and . Each of the values represents the average of left, middle, and right longitudinal profiles taken from the surface. It can be seen that is negative for all of the terrains generated, indicating that the PSD has a negative slope. The magnitude of will thus indicate how quickly the energy of the surface decreases as the wave number increases. is related to the overall energy of the surface. It can be seen that is proportional to for a given fractal dimension . Figure 3.7 shows fit lines for a longitudinal profile taken from a surface generated with for increasing scaling parameter . From this plot it can be seen that he slope of the fit line remains constant while the overall amplitude of the fit line scales with . This is to be expected since the slope of the PSD is proportional to the fractal dimension of the surface. 24 Table 3.1? PSD exponential fit constants for profiles taken from surfaces generated with various values of fractal dimension and scaling parameter . D = 2.25 D = 2.35 D = 2.5 G R k G R k G R k -3.80 -3.74 -3.71 -3.80 -3.83 -3.83 -3.73 -3.85 -3.83 -3.77 -3.74 -3.79 -3.74 -3.76 -3.76 -3.87 -3.73 -3.69 Figure 3.7 ? PSD fit exponential fit lines for a profile taken from surfaces generated with fractal dimension for various values of scaling parameter . 3.2.3. Statistical Tests for Terrain Profiles To gain a more complete understanding of the surfaces generated by the (W-M) method, various statistical tests can be performed on the profiles. The three tests which will be considered are the statistical tests for linearity, Gaussianity, and stationarity. These tests have been used by researchers to analyze experimental terrain profiles and have become accepted by the field as 25 important properties of terrains [22,30]. Each of these tests can provide different a perspective on the behavior of the terrain profiles. Applying the tests to the surfaces generated by the W-M function provides another basis for comparison between this method and other commonly used methods. One method for assessing the linearity of a time (or spatial) domain sequence is to analyze the residuals of a linear regression to the signal. Loosely speaking if the residuals of the linear regression show a non-linear trend, it can be said that the surface is non-linear. Alternatively, if the residuals of the linear regression are near zero it indicates the signal is linear. Figure 3.8 shows the linear regression residuals for profiles taken from surfaces with and various values. It can be seen that the residuals are non-linear, however if magnitude of these residuals decrease it can begin to appear more linear. Since the surfaces generated are randomly generated, some profiles may be linear and other non-linear for the same fractal parameters. 26 Figure 3.8 ? Linear regression residuals for a profile taken from surfaces generated with fractal dimension and (top) (middle) 9 (bottom) Two approaches were used to test the Gaussianity of the profiles which have been generated. The first method is a qualitative check comparing the empirical density function of the profile to the analytical probability density function based on the mean and standard deviation of the surface. The second is the Kolmogorov-Smirnov (K-S) test which was used as a means to obtain a quantitative measure of the Gaussianity of the surfaces [44]. Figure 3.9 shows the empirical density function (blue histogram) of the profile compared to the analytical probability density function of the surface. From the plots it can be determined that the empirical density function is non-Gaussian, although it does loosely resemble a Gaussian distribution. The K-S test for each of these surfaces confirms the qualitative assessment rejecting the null hypothesis of a Gaussian distribution. 27 (a) 28 (b) (c) Figure 3.9 ? Empirical distribution function compared with analytical Gaussian probability density function of profiles taken from surface with fractal dimension and (a) (b) 9 (c) . 29 The stationarity of a time series is the property which means that the statistics of the signal do not change as a function of time. In order to assess the stationarity of the profiles taken from the generated fractal surface, the windowed standard deviation of the profile was taken. Figure 3.10 shows the windowed standard deviation for profiles for surfaces where for increasing values of G. The window size in this case was 10m which is 10% of the total profile length. Figures 3.11 and 3.12 show the Allan deviation for the same profiles on a linear scale and log-log scale respectively. The Allan deviation can be particularly helpful since it shows mean standard deviation of the windows of data for an increasing window size. This is important since the profiles may be stationary for small regions of their length even if the profile as a whole is non-stationary. If the signal is stationary the standard deviation of the signal should not change significantly over the length of the signal. From these figures it is clear that the standard deviations of the profile change over its length. Thus it can be concluded from this data that the profiles generated here by using the W-M function are non-stationary. 30 Figure 3.10 ? Windowed standard deviation over the length of a profile taken from a surface generated with and (top) , (middle) 9 , (bottom) . Figure 3.11 ? Allan Deviation for profile taken from generated surface with fractal dimension and various values of scaling parameter plotted on linear scale. 31 Figure 3.12 ? Allan Deviation for profile taken from generated surface with fractal dimension and various values of scaling parameter plotted on log-log scale. 3.3 Vehicle Simulation The most important aspect of generating terrains for vehicle simulation is that the response of the vehicle when simulated on these terrains matches that of a vehicle on the real terrains. To this end it is desired to develop vehicle models which can accurately capture the vibrations which a vehicle will undergo while driving on rough terrains. As with any modeling there is a trade-off between model complexity and fidelity. This section investigates the use of vehicle models to analyze the effectiveness of the methodology for generating terrains. 3.3.1. Vehicle Suspension Models There are several suspension models which are used to simulate vehicle motions caused by the terrain roughness. The quarter car model, shown in Figure 3.13, is the simplest representation of the vehicle suspension. This model consists of a sprung mass and an un-sprung 32 mass connected by a spring and damper. This is a two degree of freedom model which models the vertical motion (heave) of the sprung mass and the motion of the un-sprung mass. A more complex representation of the suspension can be obtained by using a half car model shown in Figure 3.14. This suspension model has four degrees of freedom: the heave of the sprung mass, the angle of sprung mass, and the motion of the two un-sprung masses. There are two methods of implementing this model. The model can represent the vehicle front to rear in which case the angle representing the pitch, or the model can represent the left to right of the vehicle in which case the angle represents the roll of the vehicle. Figure 3.13 ? 2 Degree of freedom quarter car model. 33 Figure 3.14 ? 4 Degree of freedom half car model. In this work, it is desired to obtain a high fidelity suspension model which models not only the vehicle heave, but also the roll and pitch motion. Thus, the seven degree of freedom (7- DOF) full suspension model is implemented to most accurately capture the motion. Shown in Figure 3.15, the 7-DOF model has a sprung mass connected to an un-sprung mass at each corner by a spring and damper. In this implementation the tires are represented by a linear spring, although some researchers add a damper to model the hysteresis in the tires. There are four inputs, one at each corner representing the terrain displacement on the tires. The origin of the model is located at the center of gravity (CG) relative to the wheelbase by parameters and . The location of the CG is related to the track width given by the parameters and . The sprung mass is represented by , and the un-sprung mass is represented by . The index refers to the left or right side of the vehicle and the index can be 1 or 2 referring to front or rear axle of the vehicle. The spring and damping coefficients are given by and respectively. The heave motion, roll motion and pitch motion are represented by , and respectively. The motion of each un-sprung is represented by . The equations 34 of motion for the heave, roll, pitch, and un-sprung mass motions, are given by Equations (3.10) to (3.16). Figure 3.15 ? 7 Degree of Freedom Full Suspension Model. Heave Equation of Motion ? ? ? + ? ? ? ? ? (3.10) Roll Equation of Motion ? ? ? ? ? ? ? ? ? (3.11) Pitch Equation of Motion ? ? ? 35 ? ? ? ? ? ? (3.12) Front Left Un-sprung mass Equation of Motion ? ? ? ? ? (3.13) Front Right Un-sprung mass Equation of Motion ? ? ? ? ? (3.14) Rear Left Un-sprung mass Equation of Motion ? ? ? ? ? (3.15) Rear Right Un-sprung mass Equation of Motion ? ? ? ? ? (3.16) Since the model is used to simulate the vehicle motions caused by the terrain, the frequency content of the motion of the vehicle is related to the spatial frequency content of the terrain based on the vehicle velocity. Thus the gain portion of the Bode plot can be analyzed to determine how the frequencies in the terrain will propagate to the motion of the body. When studying the motion of the vehicle it is desirable to analyze states which can be measured directly. Direct measurements of the roll rate and pitch rate can be obtained by using an inertial measurement unit (IMU). It is not uncommon to find IMUs with sample rates of 100 Hz or higher. The heave position and velocity can be measured directly by GPS however the sample rate (1-10 Hz) is too slow to capture the vibration dynamics. In order to increase the update rate of these estimates the GPS can be blended with the IMU to provide estimates of these states at the sample rate of the IMU. This process is discussed in detail in Chapter 4. Figures 3.16 ? 3.20 show the gain of Bode plots for the transfer function from the inputs to the heave position, 36 velocity, roll rate, pitch rate, and suspension deflection respectively. It can be seen from these plots that there are certain ranges of frequencies for which the system will undergo resonance. These plots can also give an indication of the bandwidth frequencies for each of the variables of interest. Figure 3.16 ? Gain magnitude vs. frequency from wheel inputs ( ) to vehicle heave . 37 Figure 3.17 ? Gain magnitude vs. frequency from wheel inputs ( ) to heave rate ? . Figure 3.18 ? Gain magnitude vs. frequency from wheel input ( ) to roll rate ? . 38 Figure 3.19 ? Gain magnitude vs. frequency from wheel input ( ) to pitch rate ( )? . Figure 3.20 ? Gain magnitude vs. frequency from wheel input ( ) to suspension deflection ( ? ). 39 These figures describe the frequency dynamics of the system to a displacement input; however it is desired to relate the input frequencies to the spatial frequency content of the terrain. The relationship between terrain spatial frequency and input frequency is given by the following, (3.17) where is the spatial frequency ( ), is the wavelength in ( ) , is the velocity of the vehicle in ( , and is the oscillatory frequency in . Equation (3.17) can be used to determine the specific terrain wavelengths for which a given vehicle state will be sensitive. Consider the gain of the roll rate shown in Figure 3.18 where there is a resonant peak at approximately 15 . Therefore, at a typical off road driving speed of 4.5 10 , the terrain wavelengths near 1.8 will cause the system to resonate. Figure 3.21 shows how the terrain wavelengths relate to the gain of the roll rate for various speeds. It can be seen that the response shifts with increasing speed. As expected the faster the vehicle is traveling longer wavelengths will cause resonance. A similar methodology can be carried out for the rest of the vehicle states of interest to determine the terrain frequency content for which the vehicle will be sensitive. Considering the application to autonomous ground vehicles, a vehicle which is traveling at a certain speed can look for terrain wavelengths in the range based on its current speed. 40 Figure 3.21 ? Roll rate gain vs. terrain wavelength for various vehicle speeds 3.3.2. Simulation Comparison Perhaps the most important aspect of generating terrains for vehicle simulation is that the response of the vehicle matches that of the actual vehicle. In order to do this, several vehicle simulations were conducted to evaluate the dynamics of a vehicle traveling over the surface. For these simulations the 7-DOF suspension model introduced in the previous section was implemented in MATLAB. The simulations were also run in the software Carsim [45] to validate the 7-DOF model. Carsim provides high fidelity vehicle models which are accepted by industry as being accurately representative of vehicle dynamics. The simulation consists of a straight drive on the rough terrain with no additional inputs but the terrain. To simplify the analysis of the W-M function for generating terrains surfaces, no road bank or road grade considered in this here. Thus the large scale elevation of the terrain does not change significantly over the length of the run. 41 The MATLAB implementation of the 7-DOF suspension model required the development of the state space model for the equations of motions (3.10) ? (3.16). The state vector for these equations can thus be defined as [ ? ? ? ? ? ? ? ] (3.18) where , , and are the heave, roll and pitch respectively, and are the wheel positions. The system model matrix is given by the following ? [ ( ) ( ) ( ) ( ) ] (3.19) where for brevity certain elements of the matrix are defined by: , , , , , , , , ( ) , ( ) , , , , 42 , , ( ) , ( ) . The input model for the system can be defined by [ ] (3.20) Depending on the sensors which are being used various output models can be used. Here it is assumed that the heave position and velocity, the roll rate, pitch rate, as well as the positions of the un-sprung masses are measured directly. As such the measurement model is defined [ ] (3.21) To simulate the system presented in Equations (3.19 ? 3.21) the discretized form of these matrices must be determined. The state transition matrix for the system can be determined by calculating the matrix exponential as 43 (3.22) The discretized input matrix can be calculated by ? ? (3.23) The model can then be simulated in a ?for? loop using the following discretized model form (3.24) where is the input vector at time step k. Using this method the 7-DOF model was simulated in MATLAB and the states were compared to the simulated output from the CarSim model. For model validation the vehicle was simulated at 10mph (4.47 ) on a terrain which was generated with fractal parameters and . These values were chosen to represent reasonable off road conditions on a relatively benign terrain. The terrain was 100m long and no road grade or road bank was included in the simulation. It should be noted that all of the figures in this section are 10 second windows of the entire simulation. The initial conditions of the CarSim model were used as the initial conditions of the 7-DOF model. Also the road heights for each wheel are available from the CarSim variable list, and these road heights were used as the inputs to the 7-DOF model. The vehicle parameters which were used for CarSim model were also used for the 7-DOF model. Figure 3.22 shows the heave position (top) and heave velocity (bottom) comparing the 7-DOF model to the CarSim model. It can be seen that there is an offset in the heave position which can be attributed to the kinematics in the CarSim model which are not present in the 7-DOF model. 44 (a) (b) Figure 3.22 ? Heave motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error. 45 The roll motions and pitch motions of the CarSim model and 7-DOF model along with their errors are shown in Figures 3.23 and 3.24 respectively. It can be seen that both the angles and angular rates match well for these plots. There is however a slight offset between the pitch angles for the two different simulations. This is caused by the static ride angle created by the rear weighting of the vehicle. The CarSim model takes into account this static ride angle while the 7- DOF model does not have that level of sophistication. Figures 3.25 and 3.26 show the motions of the front and rear un-sprung mass motions respectively. The error is shown in part (b) of these figures. It can be seen that the positions and velocities have the same shape which some offset in the positions which can be attributed to suspension preloading and kinematics which are included in the CarSim model. 46 (a) (b) Figure 3.23 ? Roll motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error. 47 (a) (b) Figure 3.24 ? Pitch motion for vehicle simulations of 7 DOF model and CarSim (a) comparison (b) error 48 (a) (b) Figure 3.25 ? Front un-sprung mass motion motion (a) comparison (b) error between 7-DOF model and CarSim 49 (a) (b) Figure 3.26 ? Rear un-sprung mass motion (a) comparison (b) error between 7-DOF model and CarSim. 50 3.3.3. Vehicle Motion Based Metrics In order to develop terrain based control algorithms for the vehicle, it is required to develop metrics which can evaluate the terrain roughness based on metrics calculated from the vehicle motion. Since the vehicle motion is highly oscillatory when driving over a rough terrain, scalar metrics need to be used to represent the variation of the signal. One method for accomplishing this is to calculate a root mean square of the signal by using the equation ? ? (3.25) where is the state of interest and is the number of samples. To test the effectiveness of these metrics simulations were run on fractal surfaces generated with fractal dimension and scaling parameter ranging from , to . The vehicle was simulated on each of the terrains at speeds of 5, 10, 15, 20, and 25 mph (2.23, 4.47. 6.71, 8.94, and 11.18 ). These were chosen as reasonable speeds for an off road vehicle on rough terrain. Figures 3.27 to 3.29 show the RMS vertical acceleration, roll rate and pitch rate respectively for the various simulations runs. It can be seen that the RMS vertical acceleration exhibits an increasing trend with speed and surface roughness. The RMS roll rate generally increases with surface roughness however for a given roughness the trend does not necessarily increase with speed as evidenced by the simulation on the surface where (red line). The RMS roll rate decreases as the speed goes from 5 to 15 mph but increases as the speed continues to increase. The RMS pitch rate tends to increase and then decrease with increased speed. It is important to note that the specific pattern of increase or decrease depends on the spatial frequency content of the terrain as it relates to the frequency response of the vehicle. 51 Figure 3.27 ? RMS vertical acceleration vs. speed for vehicle simulated on surfaces with varying scaling parameter G Figure 3.28 ? RMS roll rate vs. speed for vehicle simulated on surfaces with varying scaling parameter G 52 Figure 3.29 ? RMS pitch rate vs. speed for vehicle simulated on surfaces with varying scaling parameter G As previously mentioned it is beneficial to analyze states which are directly affected by the terrain roughness and can be measured directly such as the pitch rate, roll rate and vertical acceleration. However other states of interest can be estimated by employing a GPS/INS filter as described in Chapter 4. Using the GPS/INS filter the roll angle, pitch angle, vertical velocity, and vertical position can be estimated at the sample rate of the IMU. This allows alternative methods of analyzing the roughness to be implemented. One rather obvious metric is to calculate the kinetic energy in the roll, pitch, and vertical motions. This can be accomplished by using the following equation ? ? (3.26) where is the vertical velocity, is the roll rate, and is the pitch rate. The mass, roll inertia and pitch inertia are given by , ? , and ? respectively. Figure 3.30 shows the kinetic energy of 53 the simulated vehicle for the various runs. It should be noted that the vertical axis on this plot is logarithmic. It can be seen that the overall kinetic energy experienced by the vehicle scales with the roughness of the terrain, more so than with vehicle speed, yet there is an increase in kinetic energy between 5 to 10 mph for most of the terrains. It is interesting to note that the kinetic energy of the body does not change significantly above 10 mph. Figure 3.30 ? Kinetic Energy vs. speed for vehicle simulated on surfaces with varying scaling parameter G Another method of evaluating the roughness of the surface based on the vehicle motions is to use the phase plane of various states of the vehicle. This can be beneficial for determining the relationship between vehicle states while operating on a rough terrain. In some cases the roughness of the terrain may cause motion primarily in one mode or another. The phase plane allows the distribution of the motion between the modes of motion to be analyzed. The roll rate 54 and pitch rate can be measured directly from an IMU making them an intuitive combination to analyze on the phase plane. If a GPS/INS system is used to determine the attitude states of the vehicle, the phase plane for the roll and roll rate or the pitch and pitch rate can also be used to analyze the vehicle motion. Figure 3.31 shows the phase plane plot for the pitch rate and roll rate. Figures 3.32 and 3.33 show the phase plane plots for the angle and angular rate for the roll and pitch respectively. These plots are shown for a simulation on each of the rough surfaces at 10mph. It can be seen that the as the roughness of the surface increases the points on the phase plane plots become more scattered. For each of these scatter plots an ellipse can be fit to the data, and the parameters of the ellipse, such as area, can be used to determine the roughness of the terrain. This process is outlined in more detail in Section 5.1. Figure 3.31 ? Phase plane plots of pitch rate vs. roll rate of vehicle simulated on terrains of various roughness at 10 MPH. 55 Figure 3.32 ? Phase plane plots of roll rate vs. roll angle for simulated vehicle simulated on terrains of various roughness at 10 MPH. Figure 3.33 ? Phase plane of pitch rate vs. pitch angle vehicle simulated on terrains of various roughness at 10 MPH. 56 Chapter 4: Terrain Measurement and Statistical Analysis In Chapter 3, methods were discussed to generate artificial terrains using fractals for the purpose of vehicle simulation. These methods were analyzed using common terrain roughness analysis metrics. Additionally it was shown that the surfaces generated using these methods were non-Gaussian and non-stationary, two traits the literature has found to be true for off-road terrains. The results of Chapter 3 suggest that the Weierstrass-Mandelbrot function could be an effective method for generating off road terrains which will match the behavior of actual terrains. This chapter seeks to investigate the properties of real off road terrains as a means of comparison to the terrain generation methodology presented in Chapter 3. Therefore in this chapter a terrain measurement system using a LiDAR mounted to the experimental vehicle is introduced for accurately mapping the terrains. Various analyses are performed on the measured terrains to compare with the terrain models. The analyses presented in this chapter seek to assess the roughness of the measured terrains. Additionally, a method is presented for identifying unique features on the terrain. 4.1 Terrain Measurement System The first step in analyzing terrains is to develop a system to accurately map the terrains using sensors aboard the vehicle. In this dissertation a Light Detection and Ranging (LiDAR) sensor is used to map the terrain. This sensor uses a scanning laser beam to measure ranges to objects at which the sensor is pointed. However, since the LiDAR is mounted to the vehicle, the motion of the vehicle will vastly effect the measurements taken by the sensor. One of the key 57 challenges in accurately mapping the terrain is to estimate the states of the vehicle in order to account for the change in the LiDAR position and orientation. Additionally, the state estimates must be smooth so they do not cause jumps in the terrain map which are not present in the terrain. 4.1.1. Experimental Hardware and Instrumentation The experimental vehicle, which is shown in Figure 4.1, is an ATV Corps Prowler. The Prowler is a light tactical all-terrain vehicle which has been instrumented with a plethora of sensors to measure accelerations, rotation rates, velocities, positions, suspension deflections, and steer angles. The Prowler has also been automated with actuation on the steering, throttle, and brakes. An Advantech ruggedized PC is used to handle data logging and other computational processing. The hardware primarily used for the terrain measurement system is shown in Figure 4.2. For measuring vehicle positions and velocities a Novatel ProPak v3 GPS receiver was used. This unit provides measurements at rates up to 20Hz and is capable of receiving real time kinematic (RTK) corrections to centimeter level accurate measurements of position. The Crossbow 440 is a 6 DOF inertial measurement unit (IMU) which can measure accelerations and rotation rates on three axes at sample rates up to 100Hz. Suspension deflections are measured using Celesco linear potentiometers. The potentiometers are mounted parallel to the axis of the struts and are sampled at 100 Hz. The Sick LMS-291 is a two dimensional LiDAR, which can measure the range, reflectivity, and angle to objects within its 180 degree field of view. The LMS-291 is capable of collecting data at 75Hz. Figure 4.3 shows how the LiDAR is mounted to the front of the vehicle. 58 Figure 4.1 ? Prowler ATV Experimental Test Bed Figure 4.2 ? Primary sensors used in mapping system (left) Novatel Propak v3 GPS Receiver (middle) Sick LMS 291 LiDAR (right) Crossbow 440 Inertial Measurement Unit 59 Figure 4.3 ? Mounting of LiDAR on front of Prowler ATV The data was collected to the PC using open source software called Mission Oriented Operating Suite (MOOS) [46]. This software runs a database on the computer to which all of the data from the various sensors are posted. The data present in the database can then be logged allowing the data to be collected in an efficient manner. One of the benefits of MOOS is it allows the data to be played back as if it were running real time, allowing the development of real-time algorithms without testing in real-time. For a more complete description of the sensor hardware and software specifications see Appendix C. 60 4.1.2. GPS/INS Loosely Couple Extended Kalman Filter The GPS/INS Extended Kalman Filter (EKF) is a commonly used method to blend measurements from inertial sensors with GPS measurements. As previously mentioned, inertial sensors are desirable because of their high sample rates up to 100Hz which can capture a wide range of dynamics. However it is well understood that when using inertial navigation sensors (INS) to obtain a navigation solution (position, velocity, attitude), the solution can drift quite drastically depending on the quality of the inertial sensors being used. If for example the accelerometers have constant biases the integration of those biases over time results in a linear trend in velocity, and quadratic trend in position. Incorporating GPS with the inertial sensors has several benefits over a purely INS solution. Depending on the circumstances GPS can yield an unbiased and highly accurate measurement of positions and especially of vehicle velocities. Unfortunately the GPS sample rates typically range from 1-20Hz, which is too slow to be used for capturing vehicle dynamics. By blending the GPS solution with the INS, estimates of the vehicle states can be obtained in between the GPS measurements. This takes advantage of the high update rate of the INS while limiting the effects of the INS drift over time. The blended solution also allows the accelerometer and gyroscope biases to be estimated and accounted for to improve the quality of the INS solution. Two excellent resources on this topic are by Gleason and Gebre-Egziabher [47], and Groves [48]. The implementation of the GPS/INS navigation filter with a loosely coupled architecture is characterized by a time update and measurement update as with any EKF. The time update can be thought of as the INS estimate of the navigation solution at the rate of the IMU. The accelerometers and gyroscopes are integrated over time to obtain estimates of the position, velocity, and attitude of the vehicle. The navigation state vector for the system is as follows 61 * ( ) + (4.1) where and are vectors of the respective north, east, and down positions and velocities. The attitude states (roll, pitch, and yaw) are denoted by [ ] (4.2) The last two elements in Equation (4.1) represent the three axis accelerometer and gyroscope biases respectively. The state vector can also be written in terms of error states which will be used in the measurement update as shown in Equation (4.3). * ( ) + (4.3) The first step of the time update is to update the attitude states. This is accomplished by taking the measurements from the gyroscopes subtracting off the estimated bias and rotating them into the vehicle frame, a process called mechanization. Lastly the states are updates using trapezoidal integration. These steps are given by Equations (4.4 ? 4.6). ? ( ) (4.4) [ ] (4.5) ( ? ? ) (4.6) The next step of the process is to update the velocity vector using the accelerometer measurements. First a rotation matrix is defined to transform the accelerometer measurements which are taken in the vehicle body frame to the navigation frame (north east down). The estimated accelerometer biases ( are subtracted from the measurements and multiplied by the rotation matrix from the body frame to navigation frame. The local gravity vector in the navigation frame must then be subtracted from the accelerometer measurements to yield the 62 change in velocity in the navigation frame. The change in velocity can then be integrated to update the estimate of velocity. This process is shown in Equations (4.7 ? 4.9). [ ][ ][ ] (4.7) ? ( ) (4.8) ? ? ? (4.9) Since in this implementation the positions are referenced in the same coordinate frame as the velocities, the change in the positions are simply the velocities in navigation frame. It should be noted that in some implementations the positions are referenced in a different coordinate frame than the velocities and must be transformed into the appropriate frame prior to integration. The velocities can then be integrated to yield the position estimates as shown by Equations (4.10) and (4.11). ? (4.10) ? ? (4.11) To complete the time update for a given time step, the covariance matrix must be propagated forward using the system model. The GPS/INS system model which describes the kinematic relationships between the measurements is non-linear and time variant. Thus in order to propagate the covariance forward the Jacobian of the system model must be defined as shown in Equation (4.12). 63 [ ? ( ) ? ? ] (4.12) Here the matrix each element of the matrix is a 3x3 block where is a matrix of zeros and ? is an identity matrix. and are the time constants for the Markoff process which models the bias drift for the accelerometers and gyroscopes respectively. The state transition matrix for the system can now be determined by (4.13) and the covariance for the next step can be calculated by using the equation (4.14) where is the discretized process noise matrix. The time update steps described here iterate at the rate of the IMU until a new measurement from the GPS receiver is available. When a GPS measurement is available the steps for the measurement update are executed. As previously mentioned, the EKF used is an error state filter, where the states are the difference between the GPS measured states and the INS estimated states. The measurement equation based on the error state vector (Equation (4.3)) can be given by (4.15) where the measurement matrix implies measurements of the positions and velocities and is written [ 9] (4.16) 64 The Kalman gain can then be calculated using current covariance matrix from the time update propagation and the measurement covariance matrix (4.17) (4.18) For all of the states with the exception of the attitude states, Equation (4.19) can be used to update the true states with the calculated error states. (4.19) The attitude states must be extracted from the rotation matrix. The residuals calculated for the attitude states are put into their skew symmetric matrix form and summed with the identity matrix of appropriate size and multiplied by the previous rotation matrix to obtain the updated rotation matrix. The Euler angles can then be extracted from the rotation matrix. This process is shown by Equations (4.20 ? 4.23). [ ] (4.20) ( ) (4.21) (4.22) ( ) (4.23) Finally the measurement update can be completed by updating the covariance matrix using the equation (4.24) 65 To test the effectiveness the GPS/INS filter for estimating the states required to develop a map of the terrain, data was collected while driving around a parking lot with the experimental vehicle. Figure 4.4 shows the path of the vehicle during the test, comparing the GPS position with RTK corrections to the GPS/INS estimate of the position. Figures 4.5 to 4.7 show the positions, velocities, and attitudes respectively. The errors between the GPS/INS solution and the respective references are shown in part (b) of these figures. It can be seen that the positions match the reference GPS positions well on the straights but deviate during the turns. One of the characteristics of the GPS/INS filter is that the solutions tend to jump during the measurement update. To minimize these effects the GPS/INS filter can be tuned to smooth the jumps in the solution. However, this tuning decreases the bandwidth of the filter keeping it from tracking the faster dynamics. The vehicle attitudes shown in Figure 4.7 are referenced to the measurements taken from a Septentrio PolaRx attitude system. The PolaRx uses three GPS antennas mounted to the vehicle to determine the absolute vehicle orientation. It can be seen that all of the attitude states follow the same trend as the reference although the solutions deviate in certain areas. In general, the attitude states are the most difficult to accurately determine since there is no direct measurement of these states. The some of the errors on the solutions for the attitudes can be attributed to mounting misalignment between the GPS antennas and the IMU. 66 Figure 4.4 ? Path driven during survey comparison of GPS data and GPS/INS 67 (a) (b) Figure 4.5 ? Comparison of positions using RTK GPS (blue) and GPS/INS (red) 68 (a) (b) Figure 4.6 ? Vehicle velocities comparing GPS (blue) to GPS/INS (red) 69 (a) (b) Figure 4.7 ? Attitude measurements for (a) comparison of GPS/INS estimate and Septentrio (b) errors between GPS/INS and Septentrio 70 Table 4.1 shows the mean, RMS, and maximum errors for each of the vehicle states of interest. The maximum position errors occur during the turns where the solution deviates for the reasons mentioned previously. Additionally, the position and velocity errors are near zero mean, while the attitude errors have some biases. It should be noted that the Septentrio attitude system has errors which can be significant ( ? depending on the operating conditions) and should not be thought of as a truth measurement but merely as a reference. Table 4.1 ? Summary of errors in GPS/INS solution Mean Error RMS Error Max Error The performance of the GPS/INS filter can vary greatly based on a variety of factors. Perhaps the most important of these is appropriately selecting the values for the process noise covariance matrix and the measurement noise covariance matrix . Ideally these values would come directly from sensor specifications, although they typically need to be manipulated to account for un-modeled dynamics in the system. Also much care must be taken in selecting the initial estimate of the covariance matrix . Since the GPS/INS filter is highly non-linear, the covariance can find local minima or instability points based on the current states. Another consideration in determining the performance of the filter is the quality of the bias estimates on the accelerometers and gyroscopes. If the biases are incorrectly estimated the velocity and 71 attitude states are directly affected. The attitudes states in particular are important because the rotation matrix and mechanization matrix are computed from them, so any error in these states is propagated through the rest of the states. Another observation on the performance of the GPS/INS implementation is that it decreases when driving off-road at higher speeds (greater than 15mph). It is hypothesized that during off-road driving at higher speeds the process noise varies from the process noise at slower speeds. Thus, the filter would need to be retuned at the higher speeds to optimize the performance. For more details on the issues related to GPS/INS performance consult works by Ryan [49] or Gleason and Gebre-Egziabher [47]. 4.1.3. LiDAR Terrain Mapping Using the estimated states from the GPS/INS filter and the LiDAR measurements, the terrain mapping system can be developed. The mapping algorithm was designed to allow the system to be implemented in real-time. The system executes time updates at the rate of the IMU estimating the vehicle states. When a new LiDAR scan is available the current vehicle states are passed into an algorithm which orients the LiDAR scans based on the vehicle position and orientation. The LiDAR positions are then saved into a point cloud of Cartesian coordinate data. When a new GPS measurement is available the GPS/INS measurement is executed and the estimates of the states and biases are updated. Since the state estimates tend to jump during the GPS/INS it is helpful to run a smoothing filter while building the maps. A simple implementation for this is to take a moving average of the previous state estimates. This allows the terrain map to be created without any artificial artifacts created by the filtered solution. A block diagram of the terrain mapping system is shown in Figure 4.8. 72 Figure 4.8 ? Block diagram for sensor fusion of GPS, IMU, and LiDAR for terrain mapping system This method allows 3-D Cartesian coordinate point clouds of the terrain to be generated which can be used for several purposes. Figure 4.9 shows the map of a lap around the parking lot at the National Center for Asphalt Technology (NCAT) test track. This data was collected on the prowler ATV driving the same loop around the parking lot at speeds between 6 and 8 mph. Multiple laps around the loop were driven to ensure the GPS/INS filter was able to accurately identify the accelerometer and gyroscope biases. For the purpose of plotting the data only a 300 meter longitudinal path was considered resulting in a map of just over one lap around the parking lot. A similar process was used for mapping of all the terrains which were analyzed. In these maps the red points are those of highest elevation and the blue points are those of the lowest elevation. It can be seen that the map has enough detail to locate not only the curbs on the inside of the loop but also the grassy areas on the outside of the loop. It can also be seen that the scan density is lower longitudinally in some areas. These are areas where the vehicle pitched causing the scans to be further forward or behind the normal scan location, resulting in more scan density in some areas and less scan density in others. 73 Figure 4.9 ? Point cloud map of parking lot in east north up coordinate frame Rather than resolve the terrain cloud map into a global east north up coordinate frame, it can be beneficial to keep the point clouds in path coordinates. This allows the terrain to be analyzed relative to the path the vehicle is driving. Additionally, the points from the scans which are outside of a range 5 meters on either side of the vehicle are removed. One reason for this is that the portion of the terrain outside this range is less relevant to where the vehicle is traveling. Another reason is that since the density of the scans is based on angular resolution, the scans are much less dense as they get further from the center. Put another way, small errors in the roll estimate of the vehicle result in much larger errors at the extremes of the scan width. When using a longitudinal terrain map for analyzing the terrain roughness, it can be helpful to define the height of the map relative to the vehicle rather than the global elevation. This method can be thought of as flattening the terrain and can be implemented quite simply by not accounting for the absolute elevation of the vehicle as measured by the GPS/INS system. Large elevation changes will affect the roughness metrics despite not usually being considered 74 roughness themselves. For this reason the effects of terrain grade and elevation changes are primarily neglected in the analysis performed in this chapter. The maps created for the analysis in this will be less accurate globally, but it will track the local roughness in the terrain much more accurately. Figure 4.10 is the map of a wooded area with brush on either side of a dirt trail that exits to an asphalt lot. Figure 4.10(a) shows the map in a global path coordinate frame and Figure 4.10(b) shows the map in a local coordinate frame. This example shows how the elevation change can be removed to account for the local roughness. Figure 4.11 shows the point cloud in the path coordinates a terrain with high grass. The grass can greatly affect the accuracy of the terrain map since the laser reflects off of the grass and not the ground. In both of these figures the path can be identified which suggests these maps can be used for path planning algorithms for the vehicles. 75 (a) (b) Figure 4.10 ? Point cloud map of dirt path leading to open rough asphalt area in (a) global path coordinates (b) path coordinates relative to the vehicle (flattened) 76 Figure 4.11 ? Point cloud map of high grassy terrain in path coordinate frame relative to the vehicle 4.2 Terrain Surface Analysis Six different terrains were mapped which will be analyzed in this section. Pictures and descriptions of the terrains are shown in Appendix B. The first will be identified as ?smooth asphalt? where the data was collected in a parking lot. The second was an asphalt lot with patches of varying asphalt and many small rocks scattered throughout. This area will be denoted as ?rough asphalt?. A gravel lot was also used for data collection where the gravel was packed down. This region will be referred to as ?packed gravel?. The fourth area for data collection was a gravel lot which was more loose and rutted. This area is identified by the term ?loose gravel?. A more traditional off-road terrain was selected as well with mostly dirt and rocks. This area is identified as ?dirt off-road?. The last data collection site was an off-road trail with tall grass in some areas, a dirt path, which finally transitioned onto the rough asphalt lot. The tall grass area is 77 referred to by the term ?grassy off-road? and the transitioning area is referred to by the term ?transition?. Since the terrain mapping methodology is dependent on GPS, the selection of the data collection sites was governed largely by the availability of GPS. The data collection was conducted at the National Center for Asphalt Technology (NCAT) test track located in Opelika Alabama. There were many areas at the NCAT test facility which would have been interesting to analyze but the proximity to trees and foliage made the GPS measurements too inaccurate to map the terrain. It is advantageous to collect data in long data runs and ideally the experiment would be conducted along a straight path. However, none of the data collection sites were long enough to collect the desired data length without driving on a closed loop. Thus the data was collected in a closed path in each of the areas. Some of the tools which will be used to analyze the terrains are implemented on 2-D longitudinal profiles rather than the full 3-D surface. For example the power spectral density (PSD) of a surface is a more abstract concept and is generally described as the PSD in two separate directions. Thus it is desirable to extract 2-D longitudinal profiles from the terrain maps which are generated. This is accomplished by finding the values which are within a narrow range of a desired lateral position. If the value for this range is too large, the profiles will have increased noise from the neighboring points laterally. If the value is too small the profile will have gaps where no points exist within the tolerance. Three profiles were extracted from each of terrain maps. Two of the profiles are from the left and right wheel paths and one is from center of the vehicle. It should be noted that the further away from the center of the vehicle the profiles are taken the more error which will be present from lack of resolution and the increased effects of roll dynamics. Longitudinally the profiles have a resolution of approximately 5 cm although this will change based on vehicle velocity. Figures 4.12 ? 4.17 show the 78 longitudinal profiles for each of the terrains that were measured. The terms ?left?, ?middle?, and ?right? in the legends of these plots refer to these respective profiles from the surface. From these plots it can be seen that the data elevation of the profiles stays primarily between -0.2 and 0.2 , yet the apparent roughness can vary significantly. Consider the smooth asphalt and packed gravel data in show in Figures 4.12 and 4.13. The undulations in the profiles are similar in order of magnitude. However, the more local roughness is much higher on the packed gravel surface. In Figure 4.17 the transition from the dirt path to the asphalt can be seen. Qualitatively the roughness of the asphalt (after approximately 650 m) is significantly lower than the off road section. When the profiles are plotted on the same scale it is quite easy to qualitatively determine which profiles are rougher than others. Figure 4.12 ? Longitudinal terrain profiles taken from smooth asphalt terrain map 79 Figure 4.13 ? Longitudinal terrain profiles taken from packed gravel Figure 4.14 ? Longitudinal terrain profiles taken from rough asphalt 80 Figure 4.15 ? Longitudinal terrain profiles taken from loose gravel Figure 4.16 ? Longitudinal terrain profiles taken from dirt off road path 81 Figure 4.17 ? Longitudinal terrain profiles taken from dirt path transitioning to rough asphalt After the data for the terrain maps was collected, the data analyses in the following subsections were conducted post processed. However the algorithms were developed with a real- time implementation in mind. The application of these algorithms in a real-time implementation is discussed in Chapter 6. 4.2.1. Root Mean Squared It is now desired to use a quantitative method to determine the roughness of each of the mapped terrains. Each of the profiles was analyzed using a sliding root mean squared calculation. A 100 window size was used which provides a means to determine the gradual changing roughness over the length of the profile. Shorter window sizes can be used to analyze more local effects of the roughness. The sliding windowed RMSE for the various profiles are shown in Figures 4.18 to 4.23. In legend of the figures ?left?, ?right?, and ?middle? refer to the calculations for the respective profiles. The trends of the RMSE are similar for each of the roughness profiles, 82 indicating the roughness tends to change together even if there are variations from left to right profile. It can be seen that the RMSE can vary quite significantly from the left wheel profile to the right profile. Observing the RMSE of the smooth asphalt (Figure 4.18) and comparing it to the packed gravel surface (Figure 4.19), it can be seen that the RMSE is higher in areas on the smooth asphalt surface than the packed gravel. This disagrees with the observations of the terrain profiles in which it is clear that the packed gravel is rougher than the smooth asphalt. This discrepancy is caused by one of the short-comings of the RMSE for evaluating surface roughness. Large changes in elevations over long length scales such as general road grade, can significantly affect the RMSE since the elevation is changing. This does not however indicate roughness as it is generally considered. Figure 4.18 ? RMS elevation of 2-D longitudinal road profiles from smooth asphalt 83 Figure 4.19 ? RMS elevation of 2-D longitudinal road profiles from packed gravel Figure 4.20 ? RMS elevation of 2-D longitudinal road profiles from rough asphalt 84 Figure 4.21 ? RMS elevation of 2-D longitudinal road profiles from loose gravel Figure 4.22 ? RMS elevation of 2-D longitudinal road profiles from dirt off-road 85 Figure 4.23 ? RMS elevation of 2-D longitudinal road profiles from dirt path transitioning to rough asphalt To address the weakness of the RMSE in capturing the roughness of the terrain, the RMS slope can be considered. In this way only the local changes in elevation are being considered rather than the overall change in elevation. This method is a more appropriate metric for describing the roughness of a terrain profile. Figures 4.24 and 4.25 show the RMS slope of the smooth asphalt and packed gravel terrains respectively. It can be seen here that when using the RMS slope as the metric, the packed gravel surface is rougher than the smooth asphalt which agrees with the qualitative evaluation. One caution when using the RMS slope rather than elevation is that erroneous data and noise in the profile can greatly amplify the magnitude of the RMS slope. An example can be seen by examining magnitude of the middle profile (green line) for the smooth asphalt in Figure 4.24. The elevated RMS slope is caused by including a local spike in the data near the 355m mark which can be seen in the terrain profile for this data set 86 shown in Figure 4.12. It is thus helpful to filter the data to diminish some of these effects. For this analysis a ten sample moving average filter was applied to the profile prior to performing the calculations. The data shown in Figures 4.24 and 4.25 includes this moving average filter. Figure 4.24 ? RMS slope of 2-D longitudinal road profiles for smooth asphalt 87 Figure 4.25 ? RMS slope of 2-D longitudinal road profiles for packed gravel 4.2.2. Power Spectral Density The PSD of the profiles taken from the terrain maps for the various surfaces can be computed to better understand the frequency content. As mentioned in Section 3.2.2 a qualitative inspection of the PSD can be helpful but often a curve is fit to the PSD data is used to provide a quantitative measure of the frequency content of the terrain. There are however, some practical issues which must be addressed when performing such a curve fit. Recall that the PSD fit will have some slope ( ) which will generally be negative for a terrain profile and scaling ( ). First, the scale over which the data is fit can alter the slope of the line quite significantly. If the spatial frequency lower bound is too low, the grade of the terrain profile will affect the scaling of the PSD. Near the upper bound of the spatial frequency range the noise in the data begins negatively 88 impacting the line fit. In this work 0.02 (50 wavelength) was chosen as the lower bound and 3 (0.33 wavelength) was chosen as the upper bound. In Figures 4.26 to 4.31, the PSDs for the terrain profiles for the various terrains are shown along with the mean PSD curve fit. It can be seen that the PSD trends downward with increasing spatial frequency which is as expected from the terrain profile since generally speaking the higher frequencies will have smaller amplitudes resulting in lower PSD. It can be seen that the smooth pavement surface (Figure 4.26) does not have any particular range where the PSD deviates from the trend. For the rougher surfaces however, there is a range of frequencies (around 0.1 ) where the PSD of the profile is elevated. This elevated range is responsible for the apparent roughness of the surface. The further this range shifts to higher frequencies the rougher the surface will feel. Recall that the plot shown in Figure 4.31 is for the transition from the off-road dirt path on to the rough asphalt. Thus it is expected that the roughness resulting PSD to be a combination of the two surfaces. The increased PSD towards the highest range of each of these plots can be attributed to noise in the data, as evidenced by the very similar shape. 89 Figure 4.26 ? PSD of longitudinal profiles taken from smooth asphalt map shown with mean curve fit Figure 4.27 ? PSD of longitudinal profiles taken from packed gravel map shown with mean curve fit 90 Figure 4.28 ? PSD of longitudinal profiles taken from rough asphalt map shown with mean curve fit Figure 4.29 ? PSD of longitudinal profiles taken from loose gravel map shown with mean curve fit 91 Figure 4.30 ? PSD of longitudinal profiles taken from off-road dirt map shown with mean curve Figure 4.31 ? PSD of longitudinal profiles taken from dirt path transitioning to rough asphalt map shown with mean curve fit 92 The results of the parameter fit PSD analysis for the terrain profiles being analyzed is summarized in Table 4.2. The fractal parameters determined from the PSD parameters assuming are shown in the middle two columns. The RMS elevation and slope are also shown as a reference to the roughness of the surface. One observation that can be made is that the magnitude of the slope of the PSD fit line relates to the roughness of the surface. The larger the magnitude of the slope of the PSD fit the rougher the surface appears. Since the apparent roughness of the surface is largely characterized by the elevated PSD magnitude in frequency in the range of 0.08 ? 0.15 , it agrees with intuition that the PSD fit line would have a more negative slope for these cases. The overall magnitude of the PSD, characterized by parameter , appears to be related to the slope although there is not enough data here to draw a definitive conclusion on these relationships. The available data collection locations are not sufficiently diverse to generalize for all terrains. Ultimately the overall roughness will be dependent on the combination of these parameters. One should note that the low frequency content of the profiles is primarily based on road grade and total elevation changes. Table 4.2 ? Parameters fit to PSD for the measured longitudinal profiles PSD Fit Parameters Fractal Parameters Root Mean Squared ----------- Elev. (m) Slope Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel 0.04 0.08 Dirt Off Road 0.07 0.10 Transition 93 4.2.3. Amplitude to Wavelength Another potential metric for analyzing the roughness of terrain profiles is to use the ratio of the amplitude to the wavelength. This method is introduced by Jackson as a method for analyzing rough surfaces [50]. Like the PSD this can be helpful for better understanding the frequency content in the profile. This can be determined in practice by taking the Fourier transform of the profile to obtain the amplitude and dividing by the corresponding wavelength as given by the expression (4.25) where is the amplitude of a given frequency level and is the wavelength associated with that frequency. Figure 4.32 shows the amplitude ratio for the profiles taken from the smooth asphalt and Figure 4.33 shows the amplitude ratio for profile taken from the off-road dirt surface. It can be seen that for the smooth asphalt surface is fairly flat. For the dirt off-road surface the magnitude of is higher than for the smooth asphalt. The ratio also decreases towards the higher wavelengths. To further characterize the roughness based on this method the average or maximum values can be analyzed. The values of for the various surfaces tested are shown in Table 4.3. From the results, the average and maximum both scale with increasing surface roughness. These results suggest that the amplitude to wavelength ratio can be an effective method for characterizing terrain roughness. 94 Figure 4.32 ? Amplitude to wavelength ratio for smooth asphalt surface Figure 4.33 ? Amplitude to wavelength ratio for off-road dirt 95 Table 4.3 ? Average and maximum amplitude ratios for the various terrain profiles measured. Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel Dirt Off Road Transition 4.2.4. Wavelet Based Feature Extraction In this section a methodology to identify features on the terrain is developed. The goal of feature extraction is to determine areas on the point cloud which are in some way identifiable. When viewing a point cloud it is easy to determine features which are highly distinguishable. This is demonstrated by Figure 4.34 which shows a point cloud of the smooth asphalt with four objects scattered along the path of the vehicle. However, quantitatively making this distinction is a more difficult task. Feature extraction is an issue which has been addressed in various ways by the ground vehicle and robotics community. In this work, the wavelet transform is considered for the purpose of determining unique features in the generated terrain maps. 96 Figure 4.34 ? Point cloud of smooth asphalt with objects scattered along vehicle path Wavelets are functions that decompose a signal into different frequency components. A trait of the wavelet transform is that each frequency level is transformed using a resolution matched to the scale being analyzed [51]. The wavelet transform is based on the same premise as the Fourier transform. However instead of representing the signal as a superposition of sine and cosine functions it represents the signal as a superposition of a function called a mother wavelet. There are several mother wavelet functions which can be used to perform an analysis, a few examples are shown in Figure 4.35. For a mother wavelet in the continuous domain, the scaling and translation are described by, ? ( ) (4.26) 97 where the parameters and relate to the scaling and translation of mother wavelet respectively. The coefficients of the wavelet can then be determined using the following expression, ? (4.27) These equations are written for a two dimensional time signal. Performing this transformation on surface requires analyzing the data matrix along different dimensions. The matrix is analyzed across the horizontal, vertical, and diagonals of the matrix. More details on wavelet theory can be found in a paper by Garps [51]. Figure 4.35 ? Examples of various mother wavelet functions The implementation of the wavelet transform in this work was done using the discrete wavelet transform (?dwt2?) command in Matlab. The ?Haar? mother wavelet was chosen since it is effectively a step change, thus the algorithm is attempting to identify step changes in the terrain. It was found that varying the type of mother wavelet does not have a large effect on the 98 results. The function returns the coefficient matrices for the horizontal, vertical, and diagonal orientations of the matrix for the surface being analyzed. The standard deviation of the coefficients is determined, and the coordinates of coefficients with a value higher than some scaling of the standard deviation are marked as points of interests. If the scaling of the standard deviation is increased, the point filtering will be more selective resulting in fewer points of interest. The optimal selection of the threshold will be dependent on the features which are being targeted. The wavelet transform can greatly reduce the number of required points to maintain information about the features on the terrain, yet it is more efficient for the points of interest close to each other to be marked as a feature as opposed to each individual point being regarded as a feature. To group the points of interest as features a clustering algorithm can be implemented. K-means clustering is one such algorithm which can be used to group the points of interest into features. This algorithm requires the user to select the number of desired clusters in a data set. The algorithm attempts to group the points into the desired number of clusters by minimizing the distance between the point and the nearest cluster mean. The algorithm is iterated until an optimal configuration is reached. The implementation of this function was performed using the MATLAB function ?kmeans?, which takes as an input the data points and returns the center location of the cluster. The data was analyzed in blocks over the length of the vehicle path. Each block of data was a ten meter square along the path. The points in each block are interpolated to a 5cm grid on the area before the data wavelet transform is taken. This addresses the possibility of the nature of the features changing over the length of the vehicle path and also allows the most relevant data to be analyzed at any given time step. For example, if the entire data set were analyzed at 99 once, large amounts of variation in one area of the terrain would affect the sensitivity of the algorithm in detecting changes in an area with lower variations. In a real time implementation this would also allow the blocks of data to be analyzed as they are collected. Figures 4.36 ? 4.38 show the results for running the feature extraction algorithm on the data collected on the smooth asphalt, packed gravel, and rough asphalt surfaces respectively. Plot (a) in each of these figures shows the vehicle trajectory along with the points of interested identified by the wavelet transform with the threshold set at ten times the standard deviation of the coefficients. Plot (b) in these figures shows the vehicle trajectory along with the extracted features using the k-means clustering algorithm with three clusters per block of data. 100 (a) (b) Figure 4.36 ? Smooth asphalt terrain loops showing (a) points of interest (b) extracted features. 101 (a) (b) Figure 4.37 ? Packed gravel terrain loops showing (a) points of interest (b) extracted features. 102 (a) (b) Figure 4.38 ? Rough asphalt terrain loops showing (a) points of interest (b) extracted features. 103 Most of the features which are extracted on the smooth asphalt terrain (Figure 4.36b) are along curbs in the parking lot or areas where the pavement meets grass. This is primarily what could be expected since those areas of the terrain are significantly different from the surrounding areas. For this case most of the features which are extracted are towards the sides of the path, but it can be seen that there are also some features which are identified along the path. In these areas the features being extracted by the algorithm might not be identified as being unique to an observer since the parking lot where the data was collected is nominally smooth. This is further exemplified by the data collected on the packed gravel shown in Figure 4.37. Although, subjectively the gravel lot where the data was collected has a reasonably high roughness, it is difficult to identify features which could be described as unique. The feature extraction algorithm presented here, however, identifies several features on the terrain. One key benefit this type of algorithm can have is in the development of navigation algorithms for areas where the terrain is generally non-distinctive. For example, simultaneous localization and mapping (SLAM) is a technique which relies on identifying and mapping features and using those features to calculate a navigation solution. This of course is more effective when there are many features to map. The algorithm presented here could potentially be used in SLAM algorithms to improve the performance when the environment is not what would traditionally be described as feature rich. 104 Chapter 5: Vehicle Experimental Response Chapter 4 presented several methods for evaluating the roughness of the experimental terrains. These can be helpful for the control of the vehicle by using the LiDAR to scan the terrain ahead of the vehicle and analyzing the roughness. These techniques have the potential to allow the vehicle to proactively adjust for the upcoming terrain. However, as discussed Section 3.3, the terrain roughness can also be analyzed by the motion of the vehicle. To this end it is important that the techniques developed in the previous chapters are validated and compared against the true vehicle responses. In this chapter the vehicle response based on the experimental data collection is examined. The same data which was used for mapping the terrains was also used for analyzing the vehicle dynamic response. In Section 5.1 the metrics which were developed in Chapter 3 are applied to the data collected with the Prowler ATV. Section 5.2 develops a 7-DOF suspension model based on the Prowler ATV. The response of the model is compared to the experimental vehicle response. Section 5.3 addresses the process of generating terrain models based on the measured terrains. These methods are verified using the 7-DOF suspension model. 5.1 Analysis of Vehicle Motion Metrics In Section 3.3.3 motion metrics were introduced which can be used to evaluate the roughness of the terrain based on the dynamic response of the vehicle. These metrics are used to provide a scalar metric which can ultimately be used in the control of the vehicle. As previously mentioned it is advantageous to use vehicle states which can be directly measured when possible. 105 This avoids errors in the metrics caused by processing methods. The first method which is presented is the root mean squared (RMS) of signals which can be measured by the inertial measurement unit (IMU). Evaluating this metric with experimental data it will be seen that the magnitude of this metric generally increases with the longitudinal speed of the vehicle. To account for the speed dependence of the RMS vertical acceleration, roll rate, and pitch rate, each respective signal can be divided by the instantaneous longitudinal velocity resulting in the velocity compensated form of the RMS as shown in Equation (5.1) ? ?( ) (5.1) Although the results in Section 3.3.3 suggest the increase is not strictly linear with increasing longitudinal velocity, the velocity compensation can reduce the dependence of the metric on the longitudinal velocity of the vehicle. Figure 5.1 shows the RMS vertical acceleration ( ), roll rate ( ), and pitch rate ( ) for the vehicle when driving on the packed gravel surface. A windowed RMS was used to allow the trend of the roughness over the length of the data run to be tracked. The RMS of the raw signals shown in part (a) and the RMS of the compensated signals are shown in part (b). During this experiment the path was driven at roughly 6-7 mph, the speed was then increased to approximately 12 mph. It can be seen in part (a) that when the vehicle speed increases at the 150 second mark, the magnitude of the metrics increase as well. The velocity compensated RMS metrics are shown in Figure 5.1 (b). It should be noted that the increase in the RMS vertical acceleration (top of Figure 5.1(a)) starting at 100 seconds is caused by an increase in the roughness of the surface. This is verified by the increase occurring at the same location in the compensated data as well. 106 (a) (b) Figure 5.1 ? Windowed RMS vertical acceleration, roll rate, and pitch rate for vehicle driving on packed asphalt (a) raw signal (b) velocity compensated signal 107 The velocity compensated RMS vertical acceleration, roll rate, and pitch rate for the experiments conducted on the smooth asphalt, rough asphalt, loose gravel, dirt off-road, and grassy off-road terrains are shown in Figures 5.2 ? 5.6 respectively. Recall that the data on each of these terrains was collected by driving a closed loop. The vehicle was driven by a human during the test, so there was some variation in the lateral position along the loop as well. The periodic trend seen in several of these plots is a result of the data being collected in a loop. In an attempt to isolate the terrain roughness effects, the steer inputs were adjusted as gradually as possible to limit the lateral dynamic?s effect on the IMU signals. However, inevitably the lateral dynamics will cause an increase in the RMS of the metrics. For example, it is expected that the turn induced roll of the vehicle will cause an increase in the RMS roll rate which is not associated with the terrain roughness. Figure 5.2 ? Compensated RMS vertical acceleration, roll rate, and pitch rate for the smooth asphalt 108 Figure 5.3 ? Compensated RMS vertical acceleration, roll rate, and pitch rate rough asphalt Figure 5.4 ? Compensated RMS vertical acceleration, roll rate, and pitch rate for loose gravel 109 Figure 5.5 ? Compensated RMS vertical acceleration roll rate and pitch rate for dirt off-road Figure 5.6 ? Compensated RMS vertical acceleration roll rate and pitch rate grassy off-road The results of these metrics for the data collected are summarized in Table 5.1. The trend for each of these metrics is similar. Based on the combination of the metrics it can be concluded 110 that the smooth asphalt is the least rough terrain and the grassy off-road terrain is the roughest. The rough asphalt and the packed gravel terrains have similar roughness. The RMS vertical acceleration does not change significantly for the less rough terrains. Based on the vertical acceleration the loose gravel appears to have same roughness as the packed gravel, however the roll rate and pitch rate indicate that the loose gravel terrain is rougher. It should be noted that the order of the roughness of the surfaces as determined by these metrics agrees with the qualitative assessment of the terrains during the experiments. Table 5.1 ? RMS values for IMU signals for various surfaces Root Mean Squared Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel Dirt Off-Road Grassy Off-road As shown by the data, the RMS metrics from the IMU correlate with the roughness of the terrains. This method is appropriate for analyzing the roughness of a terrain, since it accounts for the velocity effects. When considering the use of these metrics for implementation in a controller there are a couple of desired properties. It is desired that the metric relate to the overall state of the vehicle as well as the terrain roughness. Thus the velocity compensation might not be advantageous since it reduces the coupling. Another consideration when using these metrics in a vehicle controller is that it may be beneficial to combine them into a single metric. One method for combining the effects of the pertinent IMU signals is to use the phase plane as presented in Section 3.3.3. By itself, the phase plane can be used as a qualitative check for the roughness of 111 the terrain, yet it is desirable to obtain a scalar metric relating to the scatter of the phase plane. To this end least squares can be used to fit the scatter phase plane data to the equation of an ellipse. This is best accomplished using roll rate and pitch rate since they are expressed in the same units. The parameters of this ellipse can then be related to the terrain roughness and vehicle oscillatory state. The overall roughness of the terrain can be expressed by the area of the ellipse. Figure 5.7 shows the phase plane plot of the pitch rate and roll rate for the data collected on the off-road dirt terrain. The least squares best fit ellipse is plotted on top of the scattered data. Table 5.2 shows the area of the ellipse fit to the phase plane data for each of the terrains. It can be seen that the roughness of the terrains when evaluated by this metric agrees with the assessment using the RMS metrics. This is expected since both methods are based on the same signals. The near circular appearance of the ellipse is indicative of the evenly distributed roughness of the terrain. Figure 5.7 ? Phase plane plot of pitch rate and roll rate with least squares fit ellipse of data plotted for off-road dirt terrain. 112 Table 5.2 ? Area of least squares fit ellipse Elliptical Area ( ) Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel Dirt Off-road Grassy Off-road There are some special cases for which this method can be particularly useful. Consider a scenario where the left and right wheel paths are in phase. In this situation it is possible that the roll rate is much lower than would be indicative of the overall roughness of the terrain. Here it is expected that the ellipse would be eccentric with the major axis in the direction of the pitch rate. It should be noted that this would not only apply for the motion caused by terrain roughness but also the motion caused by lateral and longitudinal vehicle dynamics. Figure 5.8 shows the phase plane of roll rate and pitch rate along with the least squares best fit ellipse for data collected while performing a sine steer maneuver. It can be seen that in this case the ellipse has become more eccentric with the major axis in the direction of the roll rate. This indicates the vehicle is rolling more than it is pitching which agrees with the intuitive understanding of the maneuver. It is helpful that this behavior can be captured by the eccentricity of the ellipse resulting in a scalar metric relating to the relationship between roll rate and pitch rate. 113 Figure 5.8 ? Phase plane plot of pitch rate and roll rate with least squares fit ellipse of data plotted for vehicle performing a sine steer maneuver on smooth pavement. The primary purpose of the suspension is to absorb variations in the terrain; therefore it is reasonable to assume the motion of the suspension will be directly related to the roughness of the terrain. Since the experimental vehicle is equipped with sensors to measure the suspension deflections, this assumption can be tested using the experimental data collected. Figure 5.9 shows the measured suspension deflections for the front left and front right corners of the vehicle during a section of the data collection for the smooth asphalt (a) and the dirt off-road (b) terrains. It can be seen from these two plots that there is much more motion in the suspension when driving on the dirt off-road than on the smooth asphalt. It should also be noted that the roll dynamics of the vehicle can also affect these measurements quite significantly. This can be seen 114 in Figure 5.9 (a) between the 20 and 30 second marks. When the vehicle is turning the suspension will compress on one side and extend on the other side depending on the direction of the turn. The RMS of these signals can then be calculated to determine the roughness of the terrains based on the suspension with the caveat that some lateral and longitudinal dynamic related deflections will be captured as well. Table 5.3 shows the sum of the RMS deflections for each corner of the vehicle. The assessment of the terrain roughness using this method agrees with the other methods presented in this section. (a) 115 (b) Figure 5.9 ? Suspension deflections of the front suspension for (a) smooth asphalt (b) dirt off- road Table 5.3 ? Total RMS suspension deflections for various terrains Total RMS Deflection (cm) Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel Dirt Off-road Grassy Off-road 5.2 Comparison of Simulated and Experimental Response One of the primary goals of this dissertation is to develop methods for generating terrain models which can accurately represent the dynamics experienced by a vehicle on an actual terrain. Based on the analysis of the experimental vehicle motion provided in Section 5.1, the response of the 7-DOF model introduced in Section 3.3 can be compared to the response of the 116 experimental vehicle when driving on the various terrains. The validation of the models to the experimental data provides the basis for comparing the generated terrain models to the experimental terrain maps. The parameters for the 7-DOF suspension model were determined from a combination of physical measurements, manufacturer component specifications, and approximations based on similar vehicles. The center of gravity of the vehicle is very nearly along the centerline of the vehicle. The track width and wheel base were measured directly. Half the track width was used as values for ( ) and ( ). The vehicle was placed on scales to measure vehicle weight and front to rear weight split. From this information the vehicle mass ( ) and longitudinal location of the center of gravity ( ) and ( ). The roll inertia (? ) and pitch inertia (? ) were approximated based on the geometry of the vehicle. The spring stiffness values for the four corners ( ) were determined from the manufacturers specifications. The damping coefficients ( ) were approximated based on the values typically associated with the spring stiffness for the vehicle. The experimental vehicle has several nuances which makes some of the standard methods for parameter determination more difficult. For passenger cars the tire stiffness is typically on the order of ten times stiffer than the spring stiffness. However the springs on the experimental vehicle are stiffer than the typical passenger vehicle and the tires are softer than the typical passenger vehicle. The tire spring stiffness ( ) were approximated based on typical values associated with an ATV tire. The parameters used in the 7-DOF model are shown in Table 5.4 117 Table 5.4 ? Parameters used in 7-DOF model of experimental vehicle Parameter Value Sprung Mass ( ) Un-sprung Mass ( ) Roll Inertia ( ) Pitch Inertia ( ) Lateral CG Distance ( ) Lateral CG Distance ( ) Long. CG Distance ( ) Long. CG Distance ( ) Spring Constant ( ) Damping Constant ( ) Tire Spring Constant ( ) One consideration which must be made is the difference between the suspension kinematics of experimental vehicle as compared to the 7-DOF model. The 7-DOF model inherently makes the assumption that the springs and deflections of the suspension are in the vertical direction relative to the wheel. However, the actual deflection of the spring and damper on the experimental vehicle are offset to the inside of the vehicle and at an angle relative to the vertical. Thus, the deflection of the spring and damper is smaller than the effective deflection of the wheel relative to the body, this is known as the installation ratio. The deflection of the wheel relative to the body is the deflection used in the 7-DOF model. Therefore in order to relate the forces in the spring and damper to the effective forces required for the model, the geometry of the suspension must be considered. Figure 5.10 shows a diagram of the suspension geometry of the experimental vehicle. 118 Figure 5.10 ? Diagram of suspension geometry of experimental vehicle It can be seen from the diagram that as the lower control arm travels through its arc the motion of the wheel will be greater than vertical motion of the spring and damper . Here it is assumed that the angle through which the control arm travels is small. Using the arc length formula for each of the motion of the shock and damper and the wheel and setting them equal using the control arm angle, the following relationship can be determined (5.2) It should be noted that this expression is an approximation when relating the deflection along the spring and damper to the deflection of the wheel. It assumes that the deflection along the arc defined by is equivalent to the deflection along the axial direction of the spring and damper. The same ratio can also be used to scale the forces along the spring and damper to the vertical direction of the wheel. This is important in determining the appropriate spring and damping constants to use in the model. Thus, the stiffness of the spring on the experimental vehicle is higher than the stiffness used in the model based on this ratio. The 7-DOF model can be simulated using the methodology presented in Section 3.3. The experimentally measured terrain profile is used as the input to the vehicle model. The model 119 response is then compared to the experimental vehicle response while the terrain was being measured. The heave, heave rate, roll, and pitch from the model are compared to the GPS/INS solution. Since the IMU measures the roll rate and pitch rate of the vehicle directly, the model roll and pitch rates are compared to the signals measured from the IMU. On the experimental vehicle, the suspension deflections are measured from linear potentiometers mounted parallel to the axis of the spring and damper. The values measured from the linear potentiometer are scaled using Equation (5.2) and compared to the deflections of the model. The comparison between the experimental vehicle and model for the packed gravel surface is shown in Figures 5.11 ? 5.14. It can be seen that there is agreement between the model and experiment for the heave and roll motions. The model response is smoother than the experimental data, which is to be expected based on noise in the experimental data. As a means for quantitative comparison between the model and experimental data, the standard deviations of the states were calculated and are shown in Table 5.5. For these cases the model is within the expected errors from the GPS/INS and IMU noise for the respective comparisons. 120 Figure 5.11 ? Heave motion comparison of experiment (blue) and model (red) Figure 5.12 ? Roll motion comparison of experiment (blue) and model (red) 121 Figure 5.13 ? Pitch motion comparison of experimental (blue) and model (red) Figure 5.14 ? Suspension deflection comparison of experimental (blue) and model (red) 122 Table 5.5 ? Standard deviations of measured states for 7-DOF model and experimental data Standard Deviation 7-DOF Experimental Heave ( ) 0.33 0.34 Heave Rate ( ) 0.07 0.09 Roll ( ) 2.19 1.98 Roll Rate ( ) 2.07 2.80 Pitch ( ) 1.27 1.22 Pitch Rate ( ) 4.41 3.58 Susp. Def. L1 ( ) 0.6 0.5 Susp. Def. R1 ( ) 0.6 0.6 Susp. Def. L2 ( ) 0.7 0.5 Susp. Def. R2 ( ) 0.7 0.5 There is more relative difference between the model and experimental data for the pitch rate. One of the potential error sources for the pitch motion (Figure 5.13) is the longitudinal accelerations caused by increases in the throttle. Recall the 7-DOF model presented here does not account for the inertial effects of the center of gravity of the vehicle. This same error will be present for the roll motion of the vehicle for lateral dynamics, although the effects are diminished at lower speeds. The error in the roll rate can primarily be attributed to noise in the IMU relative to the dynamics in the roll motion. This has the effect of inflating the standard deviation of the roll rate as compared to the model. The increased dynamics in the pitch motion reduces the effect of the IMU noise on the standard deviation of the pitch rate of the experimental data. It is hypothesized that one of the primary error sources between the measured suspension deflections and the model suspension deflections is the non-linearity of the tires. Although the 7-DOF model represents the tires as a linear spring in actuality ATV tires can have non-linear behavior as well as significant damping. Additionally some of the error can be attributed to vehicle squat and oscillation during acceleration. It was determined during the testing that the experimental vehicle 123 has a tendency to oscillate under throttle particularly in the longitudinal direction at lower speeds (~5 mph). The oscillations decrease at speeds higher than approximately 8 mph. 5.3 Experimentally Based Terrain Generation The ultimate purpose of a terrain modeling method is to accurately represent the characteristics of an actual terrain. In this section the terrain model using the 3-D Weierstrass- Mandelbrot (W-M) function is compared to the experimentally measured terrains. Recall that in order to generate a fractal terrain using the W-M function, determination of the appropriate combination of fractal parameters is required. It was shown in Section 3.1 that the fractal parameters and are related to the slope and scaling of the PSD. One potential method for relating the measured terrain profiles to the desired fractal parameters is to calculate the PSD of the terrain profiles. Equations (3.4) and (3.5) can then be used to determine the fractal parameters. The fractal parameters can then be used in the 2-D W-M function to generate a terrain profile. There are two main issues with this method. First as discussed in Section 3.3.2 this method requires fitting a line to the PSD of the data. Noise in the experimental data or elevation changes can drastically affect the resulting line fit, which in turn will affect the fractal parameters. The other issue which is perhaps more problematic is that this method inherently makes the assumption that the experimental terrain data being modeled is a self-similar fractal. While the fractal assumption fits the experimental terrain profiles reasonably for certain length scales, the assumption generally breaks down for the profile as a whole. This is particularly true for the large scale elevation changes of the terrain profiles. The effect this has on the generated fractal terrain profile is that it typically cannot capture the roughness for all scales. Depending on how the line fit is conducted on the PSD, the generated profile will have one of two traits. One 124 case is that the fractal terrain will capture the roughness of the high frequency content without capturing the low frequency content. The other case is that the high frequency content will be improperly scaled by the high amplitude low frequency content resulting in a profile which is too rough. Another way to say this is that the fractal profile, although scaled differently, will tend to look the same for a given number of frequency levels. In order for this method to work for modeling the entire length scale, it must be combined with some knowledge of the original profile. For instance the Fourier transform of the original terrain profile can be used to determine the amplitudes of the lower frequency content. This content can be isolated by setting the amplitudes of the higher frequency content equal to zero. Then the inverse Fourier transform can be taken resulting in a spatial domain base terrain profile. The W-M function can then be used with random phase shifts to generate the higher frequency content on top of the base profile. Figure 5.15 shows the comparison of the experimentally measured profiles taken from the loose gravel terrain map (top) and their regenerated terrain profiles using the described method. This method has the advantage of being more closely tied to the original terrain profile. The quality of the regenerated terrain using this method will depend on appropriately determining the point at which to cut off the Fourier transform frequencies, along with how many frequency levels of the W-M function are considered. 125 Figure 5.15 ? Comparison of experimental loose gravel terrain profile (top) with regenerated partial fractal terrain profile (bottom) The preferred method for regenerating a terrain profile based on an experimental profile is based on the analysis of the 3-D W-M function performed in Section 3.2.1. It was determined that for a given fractal dimension ( ) there is an exponential relationship between the RMSE of the longitudinal terrain profiles taken from a surface and the fractal scaling parameter used to create it. This relationship was determined by analyzing generated surfaces and analyzing the RMSE of the profiles. Exponential equations were then fit to the data. These equations were shown on Figure 3.4. The equation determined for the relationship when can be solved to yield an expression for as a function of the profile RMSE as seen in Equation (5.3). ( ) (5.3) The RMSE of the measured terrain profile can be determined using the method presented in Section 4.2.1. The RMSE of the left and right wheel profiles were used to calculate two values 126 which were averaged together for the value used in the 3-D W-M function. The left and right wheel path profiles were then extracted from artificial 3-D terrain surface and compared with the terrain profiles used to generate them. It is important to note that the flattened maps were used to reduce the effect of improperly scaling the RMSE of the terrain profiles. Recall that elevation changes not typically associated with terrain roughness will artificially inflate the RMSE metric. This will result in the regenerated terrain profiles being rougher than the experimental terrain profiles. The comparison of the experimental terrain profiles and regenerated fractal terrain profiles for the dirt off-road and loose gravel terrains are shown in Figures 5.16 and 5.17 respectively. These two were chosen as test cases since the focus of this dissertation is developing methods for off-road terrains. Additionally, both of these terrain profiles had minimal large scale elevation changes. It can be seen for both of these cases that the generated fractal terrain profiles are on a very similar scale as the original terrain profile. Investigating the top part of Figure 5.16, it can be seen that there is a general elevation change in the right profile near the 30m mark. The elevation change artificially inflates the RMSE causing to be inflated. This increase is enough to cause the slightly higher overall roughness of the regenerated profiles. The key distinction between the experimental terrain profiles and the fractal profiles is that the former tends to have the phases of the right and left profiles more aligned. Since the fractal surface is randomly generated there is no guarantee that this trait will be captured for a given profile. In the bottom of Figure 5.17 it can be seen that for parts of the profile the left and right wheel paths are close to in phase. 127 Figure 5.16 ? Dirt off-road longitudinal terrain profiles from terrain map (top) compared to longitudinal terrain profiles from randomly generated 3-D surface using Weierstrass- Mandelbrot function with parameters determined from fractal parameter RMSE relationship (bottom). Figure 5.17 ? Loose gravel longitudinal terrain profiles from terrain map (top) compared to longitudinal terrain profiles from randomly generated 3-D surface using Weierstrass-Mandelbrot function with parameters determined from fractal parameter RMSE relationship (bottom). 128 To compare the frequency content of the experimentally measured and regenerated terrain profiles the power spectral densities for the profiles can be analyzed. Figures 5.18 and 5.19 show the power spectral density for the right and left profiles taken from the experimentally measured terrain and the fractal regenerated terrain. The oscillations of the PSD in the fractal surface (black markers) are caused by the increasing aspect ratio of the fractal model at higher frequencies. It can be seen that the PSD of the experimental terrain profiles match the PSD of the fractal terrain profiles reasonably well for frequencies higher than 0.2 cycles/m. The magnitudes of the PSD for the fractal terrains are higher than their experimental counterparts for the frequency range of 0.02 ? 0.1 cycles/m. As previously mentioned this range is particularly important in determining the roughness of the terrain profile. It can also be seen that the deviation in this range is higher for the loose gravel surface than the dirt off-road. Figure 5.18 ? Power spectral densities for loose gravel experimental surface and regenerated fractal surface 129 Figure 5.19 ? Power spectral densities for loose gravel experimental surface and regenerated fractal surface To determine the effectiveness of terrain profile regeneration method, 7-DOF model can be simulated on both the experimental and fractal terrains. Although the CarSim model has higher fidelity, the 7-DOF MATLAB model was chosen because it provides more control over the simulation of the vehicle terrain interaction. Since the same model was used both terrains the effects of the vehicle model error were reduced. This allows the response of the model to be analyzed to determine the similarity of the terrains with respect to the model dynamics. The heave, roll, and pitch from the simulated vehicle response on the experimental and regenerated loose gravel surfaces are shown in Figures 5.20 ? 5.22. It can be seen that the heave and pitch motions have oscillations of similar magnitudes. However, the vehicle simulated on the fractal surface experiences much more roll motion than for the experimental surface. This is caused by the left and right wheel paths being out of phase resulting in more vehicle roll. The experimental profiles tend to be more similar laterally thus the vehicle on these profiles does not exhibit the 130 increased roll motion. Another observation is that the fractal surface results in a homogenous vehicle response as compared to the real surface. Due to the non-stationary nature of the experimentally measured terrain, there are bumps and discontinuities which cause dynamics to change over the length of the run. There is a peak in each of the signals at near the 32 second mark for the experimental terrain which was caused by a bump. Figure 5.20 ? Heave motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface 131 Figure 5.21 ? Roll motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface Figure 5.22 ? Pitch motion comparison of 7-DOF model vehicle response on longitudinal profiles taken from experimental loose gravel profile and longitudinal profiles taken from regenerated fractal surface 132 As a measure of how similar the vehicle responses were, the experimental and fractal profiles the RMS of the various vehicle states was calculated. The results for the simulations on the experimental and fractal terrains at various speeds for the dirt off-road and loose gravel are summarized in Tables 5.6 and 5.7 respectively. For the loose gravel terrain the fractal generation provides good agreement with the experimental terrain except for in the roll motion for the reasons explained previously. The fractal generated surface for the dirt off-road terrain performs well, but the values are slightly elevated relative to their experimental counter parts. Again this is caused by the abrupt increase in the elevation of experimental profile resulting in and increased value of RMSE. Table 5.6 ? Summary of RMS values for 7-DOF model simulated on longitudinal terrain profiles from dirt off-road 5 MPH 10 MPH 15 MPH 20 MPH Exper. Fractal Exper. Fractal Exper. Fractal Exper. Fractal RMS heave RMS heave rate RMS roll RMS roll rate RMS pitch RMS pitch rate 133 Table 5.7 ? Summary of RMS values for 7-DOF model simulated on longitudinal terrain profiles from loose gravel 5 MPH 10 MPH 15 MPH 20 MPH Exper. Fractal Exper. Fractal Exper. Fractal Exper. Fractal RMS heave RMS heave rate RMS roll RMS roll rate RMS pitch RMS pitch rate Although this method provides a frame work for generating fractal terrains which resemble experimental terrains there are several areas which must be addressed to increase the realism of the terrains. First, as mentioned earlier in this chapter the fractal model is not effective in capturing large scale undulations while preserving the detail of the smaller scales. To more accurately represent real terrain profiles, this method needs to be combined with a methodology for generating large scale elevation changes. Additionally, the relationship used in the terrain generation methodology relies on the RMSE which was shown to not always be representative of the terrain roughness in Section 4.2.1. This method could be improved by using a similar methodology to relate the fractal parameters to the RMS slope which may be more appropriate. It was also observed in this analysis that the experimental terrain profiles tend to have similar phases for much of the data collected. To improve the terrain generation methodology it would be helpful to include more coupling between the lower frequency content of the W-M function. This could be potentially be implemented by defining the phase shifts of the surface for lower 134 frequencies while randomizing them for higher frequencies. Lastly, the experimental terrains have transients and discontinuities which cannot easily be modeled using the fractal model. Methods need to be developed which will better capture these events. 135 Chapter 6: Conclusions and Future Work 6.1 Terrain Roughness Summary The roughness of the experimentally measured terrains can be classified by the surface based metrics discussed in Chapter 4 or the motion based metrics discussed in Chapter 5. The results of the various roughness metrics for the measured surfaces are summarized in Table 6.1. The roughness of the terrain can be characterized by a combination of these metrics. Care should be taken when fitting the slope of the PSD since noise in the profile data can negatively impact the parameters. The RMS elevation is an effective metric as long as there are no large elevation changes in the profile. The RMS slope was introduced as a method to reduce the effect of the elevation changes on the metric, but it can be negatively impacted by the presence of noise in the data. Table 6.1 ? Surface and Motion based metrics for each of the terrains measured Surface Based Metrics Motion Based Metrics PSD PSD RMS Elev. RMS Slope RMS RMS RMS Area Smooth Asphalt Rough Asphalt Packed Gravel Loose Gravel Dirt Off-Road Grassy Off- Road ------ ---------- ------ ------ 136 The motion of the vehicle as measured by the inertial measurement unit or suspension deflection sensors can also be used to characterize the roughness of the terrain. The RMS vertical acceleration, roll rate, pitch rate, and suspension deflections show increasing trends with terrain roughness. These metrics can be calculated for windows of data to track the change of the roughness over the length of the surface. It was determined that the RMS roll rate and pitch rate can be impacted by the lateral and longitudinal dynamics of the vehicle. A method for combining the measured signals from the IMU into one metric based on the phase plane of the roll and pitch rates was presented. Using least squares, an ellipse can be fit to the scatter data of the phase plane. It is suggested that the parameters of this ellipse can be used as an input to a vehicle controller providing information about the terrain and current vehicle dynamic state. The area of the ellipse relates to the roughness of the terrain, while the eccentricity of the ellipse relates to increased motion in either the roll or pitch modes. 6.2 Terrain Modeling The 2-D and 3-D Weierstrass-Mandelbrot fractal functions can be used to generate terrain profiles and terrain surfaces respectively for the simulation of vehicles. An analysis of the behavior of these functions for different parameters was conducted. Many of the techniques which can be used to evaluate the terrains are more applicable to profiles rather than surfaces. Thus, longitudinal profiles were extracted from the surface and analyzed. The root mean squared elevation of the modeled terrain profiles exhibited an exponential relationship with fractal scaling parameter ( ) for a given fractal dimension ( ). The power spectral density (PSD) of the profiles was also calculated. Additionally, the exponential relationship between RMSE and as well as the PSD can be used to relate experimentally measured terrain profiles to the W-M function to generate terrains similar to experimentally measured terrains. The PSD based method 137 requires fitting a line to the PSD trend and extracting the fractal parameters from the line. However the fractal assumption breaks down over long scales of the terrain resulting in improperly scaled generated terrain profiles. A Fourier transform of the original terrain profile can be used to establish the low frequency content of the generated profile. The W-M function can then be used to generate the high frequency content of the profile. The preferred method for generating the terrain profiles is by using the exponential relationship between the fractal scaling and RMSE. This is a method can produce terrains which provide comparable results to the experimental terrain based on a vehicle simulation. However there are also nuances present in actual terrains which the W-M function fails to capture. It was suggested that methods could be developed to improve the realism of the terrains. 6.3 Terrain Mapping A methodology was developed to fuse a LiDAR with Global Positioning System (GPS) and IMU to create 3-D point cloud maps of the terrain. The GPS and IMU are blended in a loosely coupled GPS/INS architecture to provide estimates of the positions, velocities, and attitudes. The state estimates are smoothed and used to resolve the frame of LiDAR into a global coordinate frame. The map can be resolved into an east north up coordinate frame or in a coordinate frame along the vehicle path. Either of these coordinate frames can be used depending on the desired analysis. In addition the vertical component of the maps can be expressed in an absolute frame by accounting for the vehicle elevation or in a relative frame by disregarding the elevation. The latter provides a flattened version which was shown to be more appropriate for determining the roughness the terrain. The elevation changes do not generally affect the perceived roughness although they can vastly affect the metrics used to evaluate terrain roughness. 138 6.4 Future Work This dissertation has provided various tools which can be used in the design and control of autonomous vehicles for off-road applications. In the development of these vehicle systems, mobility is an important issue. The terrain roughness is just one of the factors which affect the mobility of the vehicle. Additionally the road grade, road bank, and surface properties can vastly affect the operation of the vehicle. Future work should address the implementation of road bank and surface property estimation algorithms as part of the terrain characterization. This section presents suggestions for how these tools can be used in applications for the control of the autonomous ground vehicles. 6.4.1. Real-time Analysis The analyses in this dissertation were performed on data which was collected and subsequently processed. In order for the metrics developed here to be useful for the operation of the vehicle rather than just the design, the methods must be implemented in real time. To this end many of the analyses were performed using windowed methods. Determining the metrics for shorter windows of data has some important benefits. Due to the non-stationary nature of terrains, the roughness of terrain over which the vehicle is traveling is subject to change. Using a windowed approach allows the metric to adjust to the altering terrain. The windowed approach also allows for improved processing time for algorithms which are more computationally intensive such as the wavelet based feature extraction. Another important consideration in the implementation of these methods in real-time is the processing of the LiDAR data which can be a considerable burden computationally. This is can be addressed by processing the point cloud data to the metrics and then erasing the point cloud. 139 All of the algorithms in this dissertation were developed and executed in MATLAB. In order for them to be executed in real-time they must be implemented in a compiled language such as C++. Another option for real-time implementation is a dSpace type system which compiles and runs MATLAB scripts in real-time. 6.4.2. Terrain Based Navigation and Control Ultimately it is desired to use the methodologies developed in this dissertation to aid in the control and navigation of unmanned ground vehicles. There are two general classes of terrain characterization metrics which were discussed in this dissertation. There are the surface based metrics, which rely on the map of the terrain generated by the LiDAR, and the motion based metrics which use the vehicle motion to characterize the terrain roughness. In regards to their use in the vehicle controller, the surface based metrics have the potential to be proactive since they are based on the LiDAR scans in front of the vehicle. Controllers using the motion based metrics will be reactive since the vehicle has to travel over the terrain in order to calculate them. The motion based metrics will however be more computationally efficient since they do not require point cloud processing. Ultimately the overall control strategy should use a combination of both types of terrain characterization metrics. These techniques can be applied to both tele-operated vehicles as well as autonomous vehicles. Presented below is an architecture for implementing the surface based and motion base terrain characterization metrics into the vehicle controller. Figure 6.1 shows a proposed architecture of the system for the terrain based vehicle control. The basis for the system will be the development of terrain based controller logic. This system can serve several purposes. Based on the current terrain, future terrain, weather factors, and the operator?s commanded input, the system can use model predictive control and reference scaling to send the appropriate input to the vehicle system. Additionally, this system can adjust 140 the controller gains to optimize its behavior for the current scenario. It should be noted that the vehicle controller will require both a longitudinal and a lateral controller. The methodology will be similar although the specific methods used will differ based on the nature of each problem. There will be coupling between the lateral and longitudinal controllers and this should be accounted for in the overall system design. Figure 6.1 ? Block diagram showing potential architecture for terrain based vehicle controller Implementing this controller will allow the vehicle to adjust for the changing terrain on which it is driving. Ultimately, this will enable the vehicle to complete missions off-road without getting stuck or rolling over. One of the benefits of this type of system is that it can be used for not only autonomous vehicles but also tele-operated vehicles. Incorporating estimates and measurements of the terrain roughness can provide the operator with additional information on how to control the vehicle. In this way the vehicle can avoid failures while aiding the operator. F u t u r e T e r r a i n ( R e m o t e S e n s o r y ) W e a t h e r F a c t o r s C u r r e n t T e r r a i n ( T a c t i l e M e t h o d s ) O p e r a t o r C o m m a n d e d I n p u t T e r r a i n B a s e d C o n t r o l l e r L o g i c V e h i c l e C o n t r o l l e r G a i n A d j u s t m e n t 141 Bibliography [1] Juan P. Gonzalez et al., "Using Rivet for Parametric Analysis of Robotic Systems," in Groudn Vehicle Systems Engineering and Technology Symposium, 2009. [2] G. Bekker, Introduction to Terrain-Vehicle Systems.: University of Michigan Press, 1969. [3] G. Bekker, Theory of Land Locomotion.: University of Michigan Press, 1956. [4] S. A. Shoop, "Terrain Characterization for Trafficability," Army Corps of Engineers - Cold Regions Research and Engineering Laboratory, CRREL Report 93-6 1993. [5] R.G. Alger and M.D. Osborne, "Snow Characterization Field Data Test Results," Final Report to Department of the Army, Contract No. DACA89-85-K-0002 1989. [6] J.Y. Wong, Terramechanics and Off-road Vehicles. Oxford, UK: Elsevier, 1989. [7] J.V. Stafford and D.W. Tanner, "Field Measurment of Soil Shear Strength and New Design of Field Shear Meter," in 9th Conference of the International Soil Tillage Research Organization (ISTRO), Yugoslavia, 1982. [8] R. Manduchi, A. Castano, A. Talukder, and L. Matthies, "Obsticle Detection and Terrain Classification for Autonomous Off-Road Navigation," Autonomous Robots, vol. 18, pp. 81- 102, 2005. [9] K. Iagnemma and S. Dubowsky, "Mobile Robot Rough-Terrain Control for Planetary Exploration," in ASME Biennial Mechanisms and Robotics Conference, 2000. [10] K. Iagnemma and S. Dubowsky, "Terrain estimation for high-speed-rough-terrain autonomous vehicle navigation," in SPIE - The International Society for Optical Engineering, vol. 4715, 2002, pp. 256-266. [11] K. Iagnemma, C. Brooks, and S. Dubowsky, "Visual, Tactile, and Vibration-Based Terrain Anaylsis for Planetary Rovers," in IEEE Aerospace Conference Proceedings, 2004, pp. 841- 848. [12] K. Iagnemma, F. Genot, and S. Dubowski, "Rapid Physics-based Rough-Terrain Rover Planning with Sensor and Control Uncertainty," in IEEE International conference on Robotics &Automation, 1999. [13] K. Iagnemma, H. Shibly, and S. Dubowsky, "On-Line Terrain Parameter Estimation for Planetary Rovers," in IEEE International Conference on Robotics and Automation, 2002. [14] L. Ojeda, J. Borenstein, and G. Witus, "Terrain Trafficability Characterization with a Mobile Robot," in SPIE Defense and Security Conference, Unmanned Ground Vehicle Technology VII, Orlando, FL, 2005. [15] M. Castelnovi, R. Arkin, and T. R. Collins, "Reactive Speed Control System Based on Roughness Detection," in 2005 IEEE International Conference on Robotics and Automation, 2005. [16] David J. Gorsich et al., "Terrain Roughness Standards for Mobility and Ultra-Reliability Prediction," in SAE World Congress, Detroit, MI, 2003. 142 [17] J Gavin Howe et al., "Analysis of Potential Road/Terrain Characterization Rating Metrics," in SAE Commercial Vehicle Engineering Congress and Exibition, Rosemount, IL, 2004. [18] J. Gavin Howe et al., "Further Analysis of Potential Road/Terrain Characterization Rating Metrics," in SAE Commercial Vehicle Engineering Congress and Exhibition, Chicago, IL, 2005. [19] G. A. Malmedahl, N. Dembski, G. Rizzoni, A. Soliman, and L. Disaro, "A Method for the Characterization of Off-Road Terrain Severity," in SAE Commercial Vehicle Engineering Congress and Exhibition, Chicago, IL, 2006. [20] P. Andren, "Power Spectral Density Approximations of Longitudinal Road Profiles," International Journal of Vehicle Design, vol. 40, no. 1/2/3, pp. 2-14, 2006. [21] A. W. Harrel, "Characterization of vehicle test courses by power spectra," Journal of the Mississippi Academy of Sciences, 2004. [22] M. Chaika, D. Gorsich, and T. C. Sun, "Some Statisitical Tests in the Study of Terrain Modeling," International Journal of Vehicle Design, vol. 36, no. 2/3, pp. 132-148, 2004. [23] L. Li and C. Sandu, "Modeling and Simulation of 2D ARMA Terrain Models for Vehicle Dynamics Applications," Journal of Commercial Vehicles, vol. 116, no. 7, 2007. [24] J. V. Kern and J. B. Ferris, "Characterizing 2-D Topographic Road Profiles," in ASME International Mechanical Engineering Congress and Exposition, Chicago, IL, p. 2006. [25] J. V. Kern and J. B. Ferris, "Preliminary Results for Model Identification in Characterizing 2-D Topographic Road Profiles," in Proceedings of SPIE- the International Society for Optical Engineering, vol. 6228, Orlando, FL, 2006. [26] S. M. Wagner and J. B. Ferris, "Reduced Order ARIMA Models of 2-D Terrain Proifles Using Singular Value Decomposition," in ASME International Mechanical Engineering Congress and Exposition, Seattle, WA, 2007. [27] M. W. Sayers, "On the Calculation of International Roughness Index from Longitudinal Road Profile," Transportation Research Record, vol. 1501, pp. 1-12, 1995. [28] M. W. Sayers, "Two Quarter-Car Models for Defining Road Rougness: IRI and HRI," Transportation Research Record, vol. 1215, pp. 165-172, 1989. [29] M. W. Sayers and S. M. Karamihas, The Little Book of Profiling.: University of Michigan Transportation Research Institute, 1998. [30] L. Li and C. Sandu, "Modeling of 1-D and 2-D Terrain Profiles using a Polynomial Chaos Approach," in Proceedings of the 16th International Conference of International Society for Terrain-Vehicle Systems (ISTVS), Turin, Italy, 2008. [31] R. Lee and C. Sandu, "Terrain profile modeling using stochastic partial differential equations," International Journal of Vehicle Systems, vol. 4, no. 4, 2009. [32] Q. Wang, J. Guo, and Y. Chen, "Fractal Modeling of Off-Road Terrain Oriented to Vehicle Virtual Test," Journal of Zhejiang University SCIENCE A, vol. 7, pp. 287-292, 2006. [33] C. Urmson et al., "A Robust Approach to High-Speed Navigation for Unrehearsed Desert Terrain," Journal of Field Robotics, vol. 23, no. 8, 2006. [34] P. Vemulapall and S. Brennan, "Design and Testing of a Terrain Mapping System for Median Slope Measurement," in Transportation Research Board of the National Academy of Science, 2009. 143 [35] J. Kremer and G. Hunter, "Performance of the StreetMapper Mobile LIDAR Mapping System in Real World Projects," , 2006, pp. 215-225. [36] J. V. Kern and J. B. Ferris, "Development of a 3-D Vehicle-Terrain Measurement System Part I: Equipment Setup," in Joint North America, Asia-Pacific ISTVS Conference and Annual Meeting of Japanese Society for Terramechanics, Fairbanks, AL, 2007. [37] S. M. Wagner, J. V. Kern, W. B. Israel, and J. B. Ferris, "Development of a 3-D Vehicle- Terrain Measurement System Part II: Signal Processing and Validation," in Joint North America, Asia-Pacific ISTVS Conference and Annual Meeting of Japanese Society for Terramechanics, Fairbanks, AL. [38] A. Soliman, G. Rizzoni, F. Liu N. Dembski, G. Malmedahl, and L. DiSaro, "The Development and Validation of a Terrain Severity," in Joint North America, Asia-Pacific ISTVS Conference and Annual Meeting of Japanese Society for Terramechanics, Fairbanks, AL, 2007. [39] B. B. Mandelbrot, The Fractal Geometry of Nature.: W. H. Freeman and Company, 1982. [40] D. Gorisch, "Terrain Roughness Standards for Mobility and Ultra-Reliability Predictions," in Proceedings of the SAE World Congress, Detroit, MI, 2003. [41] J. Yang and K. Komvopoulos, "A Mechanics Approach to Static Friction of Elastic-Plastic Fractal Surfaces," Journal of Tribology, vol. 127, no. 2, 2005. [42] S. Ganti and B. Bhushan, "Generalized Fractal Analysis and its Applications to Engineering Surfaces," Wear, vol. 180, pp. 17-34, 1995. [43] M. Ausloos and D. H. Berman, "A multivariate Weierstrass-Mandelbrot function," in Royal Society of London. Series A, Mathematical and Physical Sciences, vol. 400, 1985, pp. 331- 350. [44] F.J. Massey, "The Kolmogorov-Smirnov Test for Goodness of Fit," American Statistical Association, vol. 46, no. 253, pp. 68-78, 1951. [45] Mechanical Simulation. (2010) CarSim. [Online]. http://www.carsim.com [46] Oxford Mobile Robitcs Group. (2010) MOOS. [Online]. http://www.robots.ox.ac.uk/~mobile/MOOS/wiki/pmwiki.php/Main/HomePage [47] Scott Gleason and Demoz Gebre-Egziabher, GNSS Applications and Methods. Norwood, MA: Artech House, 2009. [48] Paul D. Groves, Priciples of GNSS, Inertial, and Multisensor Integrated Navigation Systems. London, UK: Artech House, 2008. [49] Jonathan Ryan, A Fully Integrated Sensor Fusion Method Combining a Single Antenna GPS Unit with Electronic Stability Control Sensors, 2011, Master's Theis, Auburn Univeristy. [50] Robert L. Jackson, "An Analytical Solution to an Archard-Type Fractal Rough Surface Contact Model," Tribology Transactions, vol. 53, pp. 543-553, 2010. [51] A. Graps, "An introduction to wavelets," Computational Science & Engineering, IEEE, vol. 2, no. 2, pp. 50-61, 1995. [52] C. Hsieh, "Robust Two-Stage Kalman Filters for Systems with Unkown Inputs," IEEE Transactions on Automatic Control, vol. 45, no. 12, 2000. [53] B. Bruscella, V. Rouillard, and M. A. Sek, "Analysis of Road Surface Profiles," Journal of Transporation Engineering, vol. 125, no. 1, pp. 247-253, 1999. 144 [54] J. J. Dawkins, D. M. Bevly, and R. L. Jackson, "Fractal Terrain Generation for Vehicle Simulation," International Journal of Vehicle Autonomous Systems, vol. 1, pp. 1-12, 2011. [55] J. J. Dawkins, D. M. Bevly, and R. L. Jackson, "Multiscale Terrain Characterization Using Fourier and Wavelet Transforms for Unmanned Ground Vehicles," in ASME Dynamic Systems and Controls Conference, Hollywood, CA, 2009. [56] G. Gerhart, R. Goetz, and D. Gorsich, "Intelligent Mobility for Robotic Vehicles in the Army After Next," in SPIE Conference on Unmanned Ground Vehicle Technology, vol. 3693, 1999, pp. 128-139. [57] V. Rouillard, B. Bruscella, and M. A. Sek, "Classification of Road Surface Profiles," Journal of Transporation Engineering, vol. 126, no. 1, pp. 41-45, 2000. [58] V. Rouillard, M. A. Sek, and B. Bruscella, "Simulation of Road Surface Profiles," Journal of Transporation Engineering, vol. 127, no. 3, pp. 247-253, 2001. [59] C. Sandu, A. Sandu, and L. Li, "Stochastic Modeling of Terrain Profiles and Soil Parameters," SAE Transactions: Jorunal of Commercial Vehicles, vol. 114, 2005. [60] Transtec. (2010) proVAL. [Online]. http://www.roadprofile.com [61] P. E. Uys, P. S. Els, and M. Thoresson, "Suspension settings for optimal ride comfort of off- road vehicles travelling on roads with different roughness and speeds," Journal of Terramechanics, vol. 44, no. 2, pp. 163-175, 2007. [62] S. Vasudevan, F. Ramos, E. Nettleton, H. Durrant-Whyte, and A. Blair, "Gaussian Process Modeling of Large Scale Terrain," Journal of Field Robotics, vol. 26, no. 10, 2009. [63] Y. Yokokohji, S. Chaen, and T. Yoshikawa, "Evaluation of Traversability of Wheeled Mobile Robots on Uneven Terrains by Fractal Terrain Model," in International Conference on Robotics & Automation, New Orleans, LA, 2004. [64] A. Y. Hata, D. F. Wolf, G. Pessin, and F. Osorio, "Terrain Mapping and Classification in Outdoor Environments Using Neural Networks," International Journal of u- and e- Service, Science and Technology, vol. 2, no. 4, 2009. [65] C. D. Crane et al., "Development of an Integrated Sensor System for Obstacle Detection and Terrain Evaluation for Application to Unmanned Ground Vehicles," , vol. 5804, Orlando, FL, pp. 156-165. [66] A. Talukder, R. Manduchi, A. Rankin, and L. Matthies, "Fast and Reliable Obstacle Detection and Segmentation for Cross-country Navigation," in IEEE Intelligent Vehicle Symposium, vol. 2 , 2002, pp. 610 - 618. [67] H. Schafer, A. Hach, M. Proetzsch, and K. Berns, "3D Obstacle Detection and Avoidance in Vegetated Off-road Terrain," in IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 823-828. [68] M.Castelnovi, R. Arkin, and T. R. Collins, "Reactive Speed Control System Based on Terrain Roughness Detection," in IEEE International conference on Robotics and Automation, pp. 891 - 896. [69] C. Wellington and A.Stentz, "Online Adaptive Rough-Terrain Navigation in Vegetation," in IEEE International Conference on Robotics and Automation, 2004. [70] V. Rouillard, B. Bruscella, and M. A. Sek, "Classification of Road Surface Profiles," Journal of Transportation Engineering, vol. 126, no. 1, pp. 41-45, 2000. [71] V. Rouillard, M. A. Sek, and B. Bruscella, "Simulation of Road Profiles," Journal of 145 Transportation Engineering, vol. 127, no. 3, pp. 247-253, 2001. [72] F. Liu et al., "A Kalman-filter-based multi-sensor terrain profile measurement system: principle, implementation and validation," in Proceedings of the SPIE Modeling and Simulation for Military Operations III, vol. 6965, 2008, pp. 69650J-69650J-9. [73] Pramod Vemulapalli and Sean Brennan, "Design and Testing of a Terrain Mapping System for Medial Slope Measurement," in Transportation Reserach Board of the National Academy of Science, 2009. 146 Appendix A: Model Based Terrain Input Estimation Presented in this appendix is a methodology for estimating the terrain over which the vehicle is driving. The 7-DOF suspension model can be used in a model based estimator to estimate the inputs to the model. Since the only inputs considered in this study are from the terrain, estimating the inputs is effectively estimating the terrain profile over which the vehicle is driving. This is a problem which is referred to in the literature as state estimation in the presence of unknown inputs. There are several approaches to this problem such as augmenting the state vector with the unknown inputs. However this requires knowledge of the underlying model driving the unknown input which is often not known. Consider the linear discrete state model with unknown inputs given by the equations (A.1) and (A.2) where is the system state vector, is the vector of unknown inputs, and is the measurement vector. is the process noise and is the measurement noise where both are zero-mean white noise sequences. Note in this implementation there are no known inputs to the system, although the method could be modified to include them. To simultaneously estimate the unknown input and vehicle states, the two-stage Kalman filter (TSKF) is implemented as presented by Hsieh [52]. The state update and covariance update equations are given by ? ? (A.3) and 147 ? ? (A.4) As implied by the name the TSKF requires a two-step process to estimate the system states and unknown inputs. The first stage is an initial prediction of the state without assuming any knowledge of the input. The time update can thus be given by the following equations for the state vector and covariance matrix respectively ? ? ? (A.5) ? ? (A.6) where is the process noise covariance matrix. The measurement update is determined by the equation ? ? ? ? ? (A.7) where ? is the Kalman gain which can be calculated as follows ? ? (A.8) where which can be thought of as the input covariance matrix is defined (A.9) Finally, the covariance matrix ( can be updated using the equation ? ? ? ? (A.10) Equations (A.5) to (A.10) represent the first stage of the estimation process for a given time step. The result from using these equations is an initial estimate of the state vector and covariance. The second step of the process is to estimate the input which is accomplished by using the following 148 ? ? (A.11) where the Kalman gain ( for the input can be calculated by the following (A.12) (A.13) Lastly, the input geometry matrix used in equations (A.3) and (A.4) is defined as follows ? ? (A.14) After each of the stages is complete for a given time step , Equations (A.3) and (A.4) can be used to update the respective state vector and covariance at time step . It should be noted that these are the discrete forms of the input and output models and . Additionally, this method makes some key assumptions about the model. First, here it is assumed that the system model is linear and time invariant, which is true for the 7-DOF model presented Section 3.3.1. One more subtle requirement is that the multiplication of the output and input matrices ( in Equation (A.13) must have intersection between the non-zero elements. Of course if there is no intersection in the non-zero elements the product will become zero and Equation (A.13) will become singular. More formally it can be said that , where or the number of unknown inputs. Also it is required that , which is to say the number of measurements must be greater than or equal to the number of unknown inputs. In relation to the 7-DOF model estimating the terrain inputs requires measurements of the un-sprung mass positions (suspension deflections) in order to satisfy these conditions. Since the roll rate and pitch rate can be measured directly from the IMU, it is intuitive to use them as measurements to the estimator. However as previously mentioned measurements of the heave position and velocity can only come from GPS which does not sample fast enough to capture the 149 vertical dynamics of the vehicle. Thus estimates of these states must be used to obtain a higher update rate to be applied as measurements to the TSKF. If estimates of a GPS/INS filter are being used as measurements to the TSKF, there is more freedom in how the measurement model can be developed for the TSKF. Roll angles and Pitch angles can be used as measurements rather than just the rates. It was determined through various simulations that the input estimation is more robust when roll and pitch angles were used rather than the roll and pitch rates. In order to test the validity of this method, the TSKF was implemented in MATLAB using the 7-DOF suspension model. The measurements for the TSKF were taken from the CarSim outputs. The measured variables were the heave, heave rate, roll, pitch, and each of the wheel positions. CarSim outputs the wheel positions directly, but in an experimental implementation they can be determined from the measured suspension deflections and the kinematic relationships discussed in Section 5.2. Figure A.1 shows the left (a) and right (b) estimates of the profiles from the TSKF compared to the actual input taken from the CarSim output variables. It can be seen that the estimated input matches the profile of the true input with the exception of a constant bias. This bias is caused from the bias which is present in the heave motion of the 7-DOF model relative to the CarSim model as seen in the simulations from Section 3.3.2. This again can be attributed to suspension preloading and kinematics which are present in the CarSim model which are not accounted for in the 7-DOF model. 150 (a) (b) Figure A.1 ? Estimation of road profile using 7DOF model based two-step Kalman filter (a) Left profile (b) Right profile 151 There are several anticipated issues with an experimental implementation of the TSKF as structured in the simulation used. First, the errors in the outputs of the GPS/INS filter are correlated, thus using them as inputs to the TSKF violates the Kalman filter?s assumptions resulting in suboptimal estimates. Another issue in an actual implementation is the varying sample rates between the measurements from the IMU and the suspension deflections. It was also determined that noise on the measurements will transfer directly through to the estimation of the input. Also, as with any model based estimation technique, the accuracy of the estimates is going to be dependent on the accuracy of the system model. Thus parameter identification for the model becomes extremely important. Yet this method has the potential to provide real-time estimates of the terrain profiles without the use of remote sensors. 152 Appendix B: Experimental Test Site Description This appendix provides images and descriptions of the locations where the experimental tests were conducted. These are given to provide a more complete understanding of the relationship of the quantitative measures presented in this dissertation to the appearance of the terrains. The experimental data was collected at the National Center for Asphalt Technology. There are a variety of terrains at this facility on which data was collected. Cut through the woods, there is an off-road vehicle course which provides a number of unique terrain types. Unfortunately the data collection in many areas of the course was limited by poor GPS coverage caused by trees and foliage. There are, however, a variety of other locations with GPS coverage that have some interesting terrain types. The smooth asphalt terrain was collected in the NCAT main parking lot. Figure B.1 shows an image taken from the top of the parking lot. This location has a quite significant elevation change from its highest point to lowest point. This was chosen as a base line case for the roughness metrics. It was also used to develop and tune the GPS/INS extended Kalman filter and terrain mapping system. 153 Figure B.1 ? Smooth Asphalt terrain from NCAT parking lot The rough asphalt terrain is shown Figures B.2 and B.3. This data was collected in an open lot which is used for storage of asphalt paving materials. This surface has patches of various asphalt types as well as rocks scattered throughout. There is a small elevation change from the highest to lowest point on the site. Figure B.2 ? Rough Asphalt terrain 154 Figure B.3 ? Rough Asphalt terrain Images of the packed gravel terrain are shown in Figures B.4 ? B.6. This is a gravel lot which appears to have been steam rolled in the past. However, as vehicles have driven on it certain areas have been churned up. Like the smooth asphalt, this terrain has a fairly large elevation change from its highest point to its lowest point. There is also an intermediate scale of elevation changes which are captured as surface roughness. 155 Figure B.4 ? Packed Gravel terrain Figure B.5 ? Packed Gravel terrain 156 Figure B.6 ? Packed Gravel terrain The loose gravel terrain is shown in Figures B.7 and B.8. This is a gravel lot which was previously used for asphalt mixing and production. As such there are a large number of rocks and asphalt chunks which are present here. This terrain is not intended to support vehicular traffic. This location was chosen since it was as a counter to the packed gravel surface. This allows the comparison between maintained gravel and unmaintained gravel to be drawn. 157 Figure B.7 ? Loose Gravel terrain Figure B.8 ? Loose Gravel terrain Figures B.9 and B.10 show images of the dirt off-road terrain where data was collected. This is a dirt road which makes a loop in an area with open sky. The road has significant undulations in certain areas and is most representative of an off-road terrain. This terrain is also nominally flat from the highest point to the lowest point. 158 Figure B.9 ? Dirt Off-Road terrain Figure B.10 ? Dirt Off-Road terrain Figures B.11 and B.12 show two perspectives of the transition from the dirt road to the rough asphalt lot. This location was used to provide a site where the roughness of the vehicle path changed significantly. The data collection here was brief since the trees limited the GPS coverage. 159 Figure B.11 ? Transition from Dirt Path to Rough Asphalt Figure B.12 ? Transition from Dirt Path to Rough Asphalt 160 Appendix C: Experimental Hardware and Software Specifications C.1 Experimental Vehicle Specifications Prowler II (Internally Transportable / Light Tactical All Terrain Vehicle) Specifications ? Power Plant : 660 cc 4-Stroke Single, Liquid Cooled, Electric Start with Auxiliary Auto - Decompression Recoil Pull ? Power Train : Fully Automatic Transmission with HI/Lo Range, Reverse and Park (Limited Slip 4WD, 4WD Diff Lock, 2WD) ? Construction : Sealed Tubular Structure with Chrome-Moly Roll Cage and Cargo Racks ? Operation : Standard Automotive Driver Controls with Sealed Rack and Pinion Steering, Console Mounted Dual Range Shifter and Parking Brake Lever 161 ? Top Speed : 63 mph (4WD, Hi Range) ? Suspension : ? Front - Independent Double Wishbone, Adjustable Preload Reservoir Shock Absorbers, 8? Travel ? Rear - Independent Double Wishbone, Adjustable Preload Reservoir Shock Absorbers, 9? Travel, Sway Bar ? Braking : ? Front - Dual Hydraulic Disc ? Rear - Shaft Mounted Hydraulic Disc with 4 piston opposed Calipers, ? Full Engine Braking on each of all 4 Drive wheels ? Payload : 1000 lb (1+ : 1 payload to weight ratio exclusive of crew) ? Towing Capacity : 2250 lb (terrain dependent) Winch : 3000 lb, with Wireless Remote Control ? Tires / Wheels : EMT Run Flat / Double Reinforced Rims ? Fuel Capacity : 16 gal. (with optional in line spare tank) ? Ground Clearance : 12.5? (13.5? @ Approach and Departure) ? Wheel Base : 75.25? Track Width : 46? front, 43? rear (center to center) ? Overall Width : 54? Overall Length : 113? ? Height : 69.5? 162 ? Weight : (dry) 1100 lb ? Ford Depth : 36? C.2 Sensor Specifications The Prowler was modified and fitted with a number of sensors for data collection and vehicle. Below is a list of the vehicle sensors which were used during the data collection and analysis for this dissertation. Sick LMS-291 LiDAR (Figure C.1) Figure C.1 ? Sick LMS 291 LiDAR ? Field of application : Outdoor ? Light source : Infrared (905 nm) ? Field of view : 90 ? ? Scanning frequency : 75 Hz ? Operating range : 0 m ? 80 m ? Max. range with 10 % reflectivity : 30 m ? Angular resolution : 0.5 ? 163 ? Heating : Optional via external heating plate ? Fog correction : yes ? MTBF : 50,000 h ? Resolution : 1 mm Crossbow IMU 440 CC (Figure C.2) Figure C.2 ? Crossbow 440 Inertial Measurement Unit ? Update Rate 2-100 Hz ? Gyroscopes (yaw, pitch, roll) - Range: Roll, Pitch, Yaw Rates ? 200 ? - Bias: Roll, Pitch, Yaw Rates 25 Hz - Random Walk < 4.5 ?/hr1/2 ? Accelerometers (x,y,z) - Range X/Y/Z ? 4 g - Bias: X/Y/Z 25 Hz - Random Walk < 1.0 m/s/hr1/2 ? Communication Protocol RS-232 Novatel GPS System (Figure C.3) Figure C.3 ? (left) Novatel GPS antenna (right) Novatel ProPak v3 receiver ? Input Voltage: 9-18 VDC ? Com Ports 1x RS-232, 2x RS-232/422, 1x USB 1.1 ? Data Sampling at 20 Hz ? Velocity Accuracy 0.03 m/s RMS ? Standalone Horizontal Position Accuracy : 1.5 m ? Standalone Elevation Position Accuracy : 165 Septentrio GPS System (Figure C.4) Figure C.4 ? Sepentrio PolaRx2 multi-antenna GPS receiver ? Standalone Horizontal position accuracy 1.1m ? Standalone Vertical position accuracy 1.9 m ? Data Sampling at 10 Hz Table C.1 ? Septentrio attitude errors assuming a 1 Hz update rate Baseline Length ( ) Heading accuracy ( ) Pitch accuracy ( ) Roll accuracy ( ) *90 Antenna separation Roll accuracy ( ) *60 Antenna separation Roll accuracy ( ) *30 Antenna separation 166 Celesco MLP-125 (Figure C.5) Figure C.5 ? (left) Celesco MLP-125 linear potentiometer (right) mounting of potentiometer parallel to suspension coilovers. ? Input Resistance 1.25K ? 10K ohms ? Full Stroke Range 0-125mm ? Output Signal Voltage Divider (potentiometer) ? Linearity : ? 0 .5 to 1 % full stroke ? Recommended Maximum Input Voltage : 42 VDC 167 C.3 Computer System The computers and hardware were mounted in a case on the front of the vehicle. Advantech Compact Embedded Computer (Figure C.6) Figure C.6 ? Advantech rugged PC ? Linux operating system (Ubuntu) ? x RS-232 ports and 5 x RS-232/422/485 ports ? Vehicle Command Codes / interfacing written in C++ using MOOS (Mission Oriented Operating System)