Inertial parameter estimation 2015-05-07 === This was a course project for 16-711 Kinematics, Dynamics, and Control at Carnegie Mellon University. The objective is to find the mass, moment of inertia in roll axis, and moment of inertia in pitch axis, and center of mass in the ground plane for a self-driving car using _only_ onboard camera and LIDAR passively just by driving around. Obviously, this isn't the best way to estimate vehicle inertial parameters, and requires making lots of assumptions, but it was fun. # Introduction With the rise in proliferation of autonomous vehicles such as the Google Self-Driving Car (#google), issues of safety are increasingly relevant and important. In particular, determining the vehicle inertial parameters is important for effective control and accurate motion simulation. These vehicles often contain a LIDAR sensor for localization and obstacle avoidance. As such, the LIDAR sensor is an obvious choice for determining said inertial parameters. Numerous studies have addressed the problem of determining inertial parameters in both the online and offline settings by using a variety of different sensors and techniques. Rajamani and Hedrick proposed in 1995 a method for inferring vehicle mass by observing the suspension, for example by directly measuring suspension travel using LVDTs (#rajamani). Several studies have investigated solving this problem in the online setting with accelerometers using various filtering techniques (#best)(#wenzel)(#vahidi)(#winstead). The present study uses _base excitation dynamics_, which has also been studied by Pence _et al_ in 2009 (#pence) using acceleromaters and in the theoretical setting by Rozyn and Zhang 2010 (#rozyn) and Kolansky and Sandu (#kolansky). My work is most similar to that of Rozyn and Zhang (#rozyn) in that it uses a known terrain profile and suspension parameters. However, to my best knowledge, there has been no study using the LIDAR sensor to provide such a terrain profile. # Model and implementation The problem statement is: Given LIDAR sensor input as point clouds sampled at discrete time steps, the vehicle geometry (wheelbase $L$ and track $B$), and suspension stiffness $k$ and damping $b$, the output is the mass $m$, the roll moment of inertia $J_r$, the pitch moment of inertia $J_p$, and the center of gravity in the plane $c_x, c_y$. The model used is a three degree of freedom base excitation model, with the state being pitch $\theta$, roll $\phi$, and bounce $Z$. It is assumed that the front and rear suspensions behave linearly and have identical parameters $k_f=k_r$, $b_f=b_r$, and that unsprung mass is negligible. Furthermore, it is assumed that the vehicle is travelling at a mostly constant velocity forwards, and that roll and pitch angles are small. No information about the yaw and lateral motion is obtained or inferred. pic http://i.imgur.com/3dDceMH.png model : Three degree of freedom base excitation model. The equations of motion are $ m\ddot Z = \sum_{i\in \lbrace fl, fr, rl, rr \rbrace} F_i \\ J_p \ddot\theta = T_p = -c_x (F_{fl} + F_{fr}) + (L-c_x) (F_{rl} + F_{rr}) \\ J_r \ddot\phi = T_r = c_y(F_{fl} + F_{rl}) - (B-c_y)(F_{fr} + F_{rr}) where the forces $F_i$ are given by a parallel spring damper approximation: $ \forall i\in\lbrace fl, fr, rl, rr\rbrace: F_i = -kZ_i - b\dot Z_i The algorithm used can be summarized as such: * The generalized iterative closest point algorithm (GICP) (#gicp) is used to determine the relative transformation between synchronized and rectified point clouds produced by the LIDAR sensor. * At each time step, the suspension travel is determined using a ray shooting method. * Inertial parameters are inferred from the suspension travel as a function of time. The implementation of the GICP algorithm used in this study is provided by the Point Cloud Library (#pcl). For ray shooting, the LIDAR sensor clearly cannot see directly beneath the vehicle. As such, the point cloud is aggregated over 100 time steps (50 before and 49 after the current timestep) of 0.1~s each for a total duration of 10~s. Vertical rays are computed from the known relative position from the wheels to the LIDAR sensor, and all points within 20~cm of each ray are obtained. The suspension travel distance is then determined by the median of those points. For inertial parameter estimation, the cost function is defined as the Euclidean distance, for all time steps, between the left and right sides of Equations 1, 2, 3, where the left side is computed from twice numerical differentiation of the pose data and the right side is obtained from Equation 4 using the ray shooting method to determine $Z_i$, where $\dot Z_i$ is computed from central differences. A simplex method provided by MATLAB's `fminsearch` function is used to search over $c_x, c_y$ to minimize this error while $m, J_p, J_r$ are computed using least squares given $c_x, c_y$. # Results pic http://i.imgur.com/aYwvDTk.jpg lidar : LIDAR point cloud accumulated over 2 seconds. Note that 10 seconds are used in actual experimentation (but it is not possible to plot 10 seconds of data due to memory limitations). pic zz.png zz : Vertical suspension travel over 444 seconds for all four wheels. pic mz.png mz : Left and right hand sides of Equation 1 over 4440 data points. The algorithm is tested on the KITTI data set (#kitti)(#kittii)(#kittiii), namely Sequence 00 of the Odometry data. The LIDAR point clouds are provided at a rate of 10~Hz, with about $1.3\times 10^5$ points per timestep. The sensor is a Velodyne HDL-64E mounted atop a Volkswagen Passat B6 station wagon. It is known that for this vehicle, $L=2.71$~m and $B=1.60$~m. Assuming that $k\approx 5\times 10^4$~N/m and $b\approx 2\times 10^3$~kg/s, and using $(c_x, c_y) = (1.3, 0.8)$~m as an initial guess, then the output of the program is: $ J_p = 3.8\times 10^3~\text{kg m}\\ J_r = 3.4 \times 10^3~\text{kg m}\\ m = 1.7 \times 10^3~\text{kg}\\ c_x = 1.1~\text{m}\\ c_y = 0.9~\text{m} These look like reasonable values since the mass is expected to be over 1400~kg (#passat) and the moment of inertia in the pitch axis is clearly much higher than the one in the roll axis. The center of mass is close to the center of the vehicle and is closer to the front than the rear, which is expected since the car is front-engined. # Discussion From Figure 4, it is clear that the data is extremely noisy. This could significantly affect the performance of the algorithm. The noise may arise from the fact that the assumptions in the model might not hold if the vehicle is being driven aggressively, or if there are nonlinear or hysteresis behaviour in the suspension. The KITTI dataset's data rate of 10~Hz is too low to capture high frequency behaviour in the suspension. Using an inertial measurement unit or a high framerate camera, the pose of the vehicle can be more accurately determined with finer temporal resolution, allowing for better estimates of acceleration. # Conclusion An algorithm is presented for the novel application of an onboard LIDAR sensor to the problem of determining the mass, center of mass in the plane, and moments of inertia in the roll and pitch axes. Testing on data on a Volkswagen Passat B6 equipped with a Velodyne HDL-64E sensor resulted in values close to expectations, despite the noisy nature of the data. * [#rajamani] Rajamani, Rajesh, and J. Karl Hedrick. "Adaptive observers for active automotive suspensions: theory and experiment." _Control Systems Technology, IEEE Transactions on_ 3.1 (1995): 86-93. * [#best] Best, Matt C., T. J. Gordon, and P. J. Dixon. "An extended adaptive Kalman filter for real-time state estimation of vehicle handling dynamics." _Vehicle System Dynamics_ 34.1 (2000): 57-75. * [#wenzel] Wenzel, Thomas A., et al. "Dual extended Kalman filter for vehicle state and parameter estimation." _Vehicle System Dynamics_ 44.2 (2006): 153-171. * [#vahidi] Vahidi, Ardalan, Anna Stefanopoulou, and Huei Peng. "Recursive least squares with forgetting for online estimation of vehicle mass and road grade: theory and experiments." _Vehicle System Dynamics_ 43.1 (2005): 31-55. * [#winstead] Winstead, Vincent, and Ilya V. Kolmanovsky. "Estimation of road grade and vehicle mass via model predictive control." _Control Applications, Proceedings of 2005 IEEE Conference on._ IEEE, 2005. * [#pence] Pence, Benjamin L., Hosam K. Fathy, and Jeffrey L. Stein. "Sprung mass estimation for off-road vehicles via base-excitation suspension dynamics and recursive least squares." _American Control Conference. ACC'09._ IEEE, 2009. * [#rozyn] Rozyn, Matthew, and Nong Zhang. "A method for estimation of vehicle inertial parameters." _Vehicle system dynamics_ 48.5 (2010): 547-565. * [#kolansky] Kolansky, Jeremy, and Corina Sandu. "Real-time parameter estimation study for inertia properties of ground vehicles." _Archive of Mechanical Engineering_ 60.1 (2013): 7-21. * [#google] Google Self-Driving Car Project, \\ `https://plus.google.com/+GoogleSelfDrivingCars/`. Retrieved May 7, 2015. * [#gicp] Segal, Aleksandr, Dirk Haehnel, and Sebastian Thrun. "Generalized-ICP." _Robotics: Science and Systems._ Vol. 2. No. 4. 2009. * [#pcl] Rusu, Radu Bogdan and Steve Cousins. "3D is here: Point Cloud Library (PCL)." _IEEE International Conference on Robotics and Automation (ICRA)_ May 9, 2011. * [#kitti] Geiger, Andreas, Philip Lenz, and Raquel Urtasun. "Are we ready for Autonomous Driving? THe KITTI Vision Benchmark Suite." _Conference on Computer Vision and Pattern Recognition (CVPR)._ 2012. * [#kittii] Geiger, Andreas, Philip Lenz, Cristoph Stiller, and Raquel Urtasun. "Vision meets Robotics: The KITTI Dataset." _International Journal of Robotics Research (IJRR)._ 2013. * [#kittiii] Fritsch, Jannik, Tobias Kuehnl, and Andreas Geiger. "A New Performance Measure and Evaluation Benchmark for Road Detection Algorithms." _International Conference on Intelligent Transportation Systems (ITSC)._ 2013. * [#passat] Technical characteristics: Volkswagen - Passat Variant (B6). \\ `http://www.auto-data.net/en/?f=showCar&car_id=8901`. Retrieved May 7, 2015.