Inertial parameter estimation

2015-05-07

  1. 1 Introduction
  2. 2 Model and implementation
  3. 3 Results
  4. 4 Discussion
  5. 5 Conclusion

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.

1 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 bestwenzelvahidiwinstead. 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.

2 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.

model
FIGURE 1 Three degree of freedom base excitation model.

The equations of motion are

1 \displaystyle{\begin{align}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})\end{align}}

where the forces F_i are given by a parallel spring damper approximation:

2 \displaystyle{\begin{align}\forall i\in\lbrace fl, fr, rl, rr\rbrace: F_i = -kZ_i - b\dot Z_i\end{align}}

The algorithm used can be summarized as such:

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.

3 Results

lidar
FIGURE 2 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).
zz
FIGURE 3 Vertical suspension travel over 444 seconds for all four wheels.
mz
FIGURE 4 Left and right hand sides of Equation 1 over 4440 data points.

The algorithm is tested on the KITTI data set kittikittiikittiii, 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:

3 \displaystyle{\begin{align}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}\end{align}}

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.

4 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.

5 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.