Road Profile Estimation Using a 3D Sensor and Intelligent Vehicle

Autonomous vehicles can achieve accurate localization and real-time road information perception using sensors such as global navigation satellite systems (GNSSs), light detection and ranging (LiDAR), and inertial measurement units (IMUs). With road information, vehicles can navigate autonomously to a given position without traffic accidents. However, most of the research on autonomous vehicles has paid little attention to road profile information, which is a significant reference for vehicles driving on uneven terrain. Most vehicles experience violent vibrations when driving on uneven terrain, which reduce the accuracy and stability of data obtained by LiDAR and IMUs. Vehicles with an active suspension system, on the other hand, can maintain stability on uneven roads, which further guarantees sensor accuracy. In this paper, we propose a novel method for road profile estimation using LiDAR and vehicles with an active suspension system. In the former, 3D laser scanners, IMU, and GPS were used to obtain accurate pose information and real-time cloud data points, which were added to an elevation map. In the latter, the elevation map was further processed by a Kalman filter algorithm to fuse multiple cloud data points at the same cell of the map. The model predictive control (MPC) method is proposed to control the active suspension system to maintain vehicle stability, thus further reducing drifts of LiDAR and IMU data. The proposed method was carried out in outdoor environments, and the experiment results demonstrated its accuracy and effectiveness.


Introduction
In the last three decades, driverless technology has rapidly developed due to people's willingness to improve living standards, and the need to improve work efficiency. The key issue to driverless vehicles is how to drive to a given place without any traffic accidents. The localization and perception of autonomous vehicles are two significant technological aspects to solve the above issue. Most studies on driverless vehicles used multi-sensors such as inertial measurement units (IMUs), light detection and ranging (LiDAR), and global navigation satellite systems (GNSSs) to achieve vehicle perception and localization [1][2][3][4][5]. Gao [6] proposed a robust localization system that made better use of the relative merits of LiDAR and global positioning systems (GPSs) to correct the data obtained by inertial navigation system (INSs) selectively in outdoor and indoor environments. Experiment results showed that the proposed localization system could maintain meter-level localization accuracy within the experimental period. Wan [7] proposed a robust and precise localization method that could maintain centimeter-level navigation accuracy in some challenging environments. The proposed system used a Kalman filter (KF) algorithm to fuse the data obtained by physical sensors such as GPS, INS, and LiDAR to achieve high localization accuracy and robustness. Wolcott [8] proposed a robust localization system comfort and handling stability were also improved with the proposed method. Theunissen et al. [28] proposed an explicit model predictive control method that used the road information obtained by LiDAR as the signal input to control the active suspension system. Experiment results showed that the heave and pitch acceleration of the sprung mass was reduced compared with the passive suspension system and skyhook controllers. The proposed algorithm also required less memory space compared with other methods, because the complex calculation process was operated offline. Du [29] proposed an output feedback control method for vehicle suspension systems that used a nonlinear extended state observer to estimated model uncertainties. The proposed approach could effectively estimate the nonlinear dynamics and mismatch disturbances such as sprung mass, unknown friction coefficient, and measurement noise caused by sensors. Results showed that vehicle ride comfort was significantly improved, and the proposed algorithm could achieve fast convergence, which indicated that the method was more applicable in engineering practice. Sun [30] investigated an active suspension controller that considered the impact of actuator input delay and band with constraints. Experiment results showed that the proposed approach achieved better vehicle performance compared with the traditional approach for active suspension systems. Pan [31] proposed an output feedback finite-time controller for vehicle active suspension systems. The proposed method used a robust first-order sliding mode observer to compensate for the model uncertainties such as mismatch disturbances, unknown parameter, and unmodeled dynamics. Results showed that the proposed controllers could effectively decrease root-mean-square errors of the vehicle vertical velocity and acceleration.
Although the aforementioned methods on road profile estimation could achieve good effects in some scenes, they could cause large errors for a vehicle working on complex uneven terrain. There are also a few scholars who conducted detailed studies on autonomous vehicles driving on complex terrain. For the above motivation, we propose a method for road profile estimation and active suspension control on complex uneven terrain. In Section 2, we introduce the configuration of our autonomous vehicle and the overall flow of our approach. In Section 3, details about the terrain profile estimation method are shown. In Section 4, we propose a method for model predictive control (MPC) with a preview, which uses terrain profile information and IMU/GPS data to control the active suspension system. Lastly, real-world experiments were carried out to demonstrate the accuracy and effectiveness of our approach.

System Structure
Autonomous vehicles working on uneven terrain need to consider not only the surrounding environment but also terrain profile information, which is different from autonomous vehicles driving on urban even driveways. Accurate road profile information can be used as a reference index for vehicle-path planning. However, the traditional autonomous vehicles experience violent vibrations when driving on uneven terrain, which leads to drifts of LiDAR and IMU data. Therefore, how to obtain accurate terrain information and drive autonomous vehicles stably and safely on complex roads are two significant issues. In order to solve the above issue, our vehicle was equipped with two GPSs, an IMU, two 3D laser scanners, and an active suspension system, as shown in Figure 1. Location accuracy is a prerequisite for constructing an accurate road profile map. Therefore, our system used real time kinematic (RTK) technology to ensure centimeter-level positioning accuracy of GPS. Our vehicle also serves for open outdoor scenes, so the stability of the positioning system is well-assured. With accurate positioning information, cloud data points obtained by LiDAR were added to the terrain elevation map and further processed by the Kalman filter algorithm. Ultimately, the elevation map information and IMU/GPS data were used as inputs to control the active suspension system to maintain vehicle stability; Figure 2 shows the main steps.

Elevation Map Estimation
Constructing a reliable and accurate elevation map needs to consider the systematic error of sensor measurements. In this section, we propose a method for constructing a probability elevation map. Elevation information in each cell of the map includes not only the estimated evaluation value, but also the confidence boundary of the estimated value. Our method is different than other proposed methods (proposed in [17]) for probability map estimation because our autonomous vehicle has an active suspension system and a global positioning system that can ensure the stability and positioning accuracy of the vehicle, thus further reducing sensor drifts.

Coordinate System Definition
Seven coordinate systems were defined in our autonomous vehicle, namely East-North-Up (ENU) coordinate system G , inertial coordinate system W , map coordinate system M , vehicle

Elevation Map Estimation
Constructing a reliable and accurate elevation map needs to consider the systematic error of sensor measurements. In this section, we propose a method for constructing a probability elevation map. Elevation information in each cell of the map includes not only the estimated evaluation value, but also the confidence boundary of the estimated value. Our method is different than other proposed methods (proposed in [17]) for probability map estimation because our autonomous vehicle has an active suspension system and a global positioning system that can ensure the stability and positioning accuracy of the vehicle, thus further reducing sensor drifts.

Coordinate System Definition
Seven coordinate systems were defined in our autonomous vehicle, namely East-North-Up (ENU) coordinate system G , inertial coordinate system W , map coordinate system M , vehicle

Elevation Map Estimation
Constructing a reliable and accurate elevation map needs to consider the systematic error of sensor measurements. In this section, we propose a method for constructing a probability elevation map. Elevation information in each cell of the map includes not only the estimated evaluation value, but also the confidence boundary of the estimated value. Our method is different than other proposed methods (proposed in [17]) for probability map estimation because our autonomous vehicle has an active suspension system and a global positioning system that can ensure the stability and positioning accuracy of the vehicle, thus further reducing sensor drifts.

Coordinate System Definition
Seven coordinate systems were defined in our autonomous vehicle, namely East-North-Up (ENU) coordinate system G, inertial coordinate system W, map coordinate system M, vehicle coordinate system B k , IMU coordinate system I, local map coordinate system M k , and LiDAR coordinate system L, respectively, as shown in Figure 3.
Suppose that the inertial coordinate system W is fixed in the real environment. Vehicle coordinate system B k , IMU coordinate system I, and LiDAR coordinate system L are fixed in the corresponding position of the vehicle, respectively, and their position relationship can be obtained through calibration technology. Map coordinate system M is also fixed in the real environment, as is the inertial coordinate system W. Local map coordinate system M k moves with the motion of the vehicle and maintains an invariant transformation with vehicle coordinate system B k .
Sensors 2020, 20, x FOR PEER REVIEW 5 of 17 coordinate system k B , IMU coordinate system I , local map coordinate system k M , and LiDAR coordinate system L , respectively, as shown in Figure 3. Suppose that the inertial coordinate system W is fixed in the real environment. Vehicle coordinate system k B , IMU coordinate system I , and LiDAR coordinate system L are fixed in the corresponding position of the vehicle, respectively, and their position relationship can be obtained through calibration technology. Map coordinate system M is also fixed in the real environment, as is the inertial coordinate system W . Local map coordinate system k M moves with the motion of the vehicle and maintains an invariant transformation with vehicle coordinate system k B .

Uncertain Estimation of Location System
The motion of the vehicle can create noise errors because of systematic errors caused by GPS/IMU, which influence the precision of the elevation map. Therefore, errors caused by GPS/IMU need to be considered when constructing the elevation map. A sub-coordinate system was built to describe the coordinate transformation relationship, as shown in Figure 4. Measurement value P expressed in coordinate system k B at time k can be converted to map coordinate system M : Where ,  r k and , Φ  k represent the covariance of the pose estimation between the inertial coordinate system W and the vehicle coordinate system k B .
Then, the error propagation of the location system can be calculated as follows [17]: ,k , ,k , , , , With Equation (1), the Jacobian matrices can be obtained:

Uncertain Estimation of Location System
The motion of the vehicle can create noise errors because of systematic errors caused by GPS/IMU, which influence the precision of the elevation map. Therefore, errors caused by GPS/IMU need to be considered when constructing the elevation map. A sub-coordinate system was built to describe the coordinate transformation relationship, as shown in Figure 4. Measurement value P expressed in coordinate system B k at time k can be converted to map coordinate system M: where Φ −1 B k W ( B k R B k P ) represents the rotation transformation of vector R B k P from coordinate system B k to coordinate system W.
The Gaussian probability distribution of R WB k and Φ WB k can be defined as follows: where r,k and Φ,k represent the covariance of the pose estimation between the inertial coordinate system W and the vehicle coordinate system B k . Then, the error propagation of the location system can be calculated as follows [17]: With Equation (1), the Jacobian matrices can be obtained: Sensors 2020, 20, 3676 6 of 17 The error propagation of the measurement system can be calculated as follows [17]: Where ,  M k represents the covariance matrix of the LiDAR measurement model, which can be obtained through sensor calibration technology. The Jacobian matrix can be obtained according to Equation (5): The truncation error needs to be considered when the cloud data points are added to the corresponding grid cell of the elevation map. In order to reduce computation, the truncation error is set as an invariant value: Therefore, covariance matrix P  of each grid cell can be defined as follows: In summary, according to Equations (3), (6), and (9), the error propagation model of an autonomous vehicle system can be expressed as follows:

Uncertain Estimation of LiDAR System
From Figure 3, measurement point P k is generally represented by vector L R LP k in LiDAR coordinate system L. With the transformation from LiDAR coordinate system L to vehicle coordinate system B k , the vector B k R B k P k could be obtained: The error propagation of the measurement system can be calculated as follows [17]: where M,k represents the covariance matrix of the LiDAR measurement model, which can be obtained through sensor calibration technology. The Jacobian matrix can be obtained according to Equation (5): The truncation error needs to be considered when the cloud data points are added to the corresponding grid cell of the elevation map. In order to reduce computation, the truncation error is set as an invariant value: Therefore, covariance matrix P of each grid cell can be defined as follows: In summary, according to Equations (3), (6), and (9), the error propagation model of an autonomous vehicle system can be expressed as follows:

Elevation Map Update Method
Cloud data points obtained by LiDAR at different times may be added to the same cell of the elevation map with the motion of the vehicle, so an effective algorithm is needed to fuse the cloud data points of the same cell. In this study, the Kalman filter algorithm was used to fuse the cloud data Sensors 2020, 20, 3676 7 of 17 points, which not only improved measurement accuracy, but also effectively utilized the height value variance of each measurement point.
Suppose that the height measurement value and corresponding covariance matrix of a cell areĥ and σ 2 h , respectively, and the height estimation value and corresponding variance matrix are h k and σ 2 h,k , respectively. The update process of the Kalman filter can be calculated: With Equation (11), the following equation can be obtained: Sometimes, in the elevation value in the same cell, large drops occur due to vertical objects in the surrounding environment. If the elevation value of the cell is fused by a Kalman filter without any judgment, the updated elevation value is far from the real value. In order to avoid the above case, this study used Mahalanobis distance to judge all measured values before elevation information fusion. Only when the Mahalanobis distance between the estimated elevation value and the new measured value is less than threshold value c is the new measured value added to the cell. The Mahalanobis distance criterion is defined as follows: (1) Ifĥ > h k+1 and d m (Z k , Z k+1 ) > c, then: (2) Ifĥ < h k+1 and d m (Z k , Z k+1 ) > c, then: (3) If d m (Z k , Z k+1 ) < c, then the new measured value can be calculated with Equation (12). In summary, this section proposed a method for elevation map estimation that considers systematic errors caused by GPS/IMU, LiDAR, and map cells. The error-propagation model of vehicle systems was defined to calculate the confidence boundary of the estimated elevation value. Besides, the Kalman filter algorithm and Mahalanobis distance criterion were used to fuse the cloud data points of the same cell. The main flow of elevation map estimation is shown in Figure 5. In summary, this section proposed a method for elevation map estimation that considers systematic errors caused by GPS/IMU, LiDAR, and map cells. The error-propagation model of vehicle systems was defined to calculate the confidence boundary of the estimated elevation value. Besides, the Kalman filter algorithm and Mahalanobis distance criterion were used to fuse the cloud data points of the same cell. The main flow of elevation map estimation is shown in Figure 5.

Model Predictive Control with Preview
Our autonomous vehicle was equipped with an active suspension system for solving the problem of vehicle stability when driving on complex uneven terrain, which could be sampled as a nine-degree-of-freedom vehicle model, as shown in Figure 6. The vehicle model consisted of six vertical unsprung masses and degrees of freedom due to the pitch, roll, and vertical motion of the mass center.
According to Newton's second law, the vertical motion of the body centroid, the pitching rotation, and the roll rotation equations can be obtained as: Where i F is the force generated by the actuator; Z , the vertical displacement of vehicle centroid; θ , the pitch angle of vehicle centroid; ϕ , the roll angle of vehicle centroid; and XX J and YY J , the moments of inertia of the X and Y axes, respectively. r L and f L are the distance between the center of mass, and front and rear suspension, respectively. a L and b L are the distance between center of mass, and left and right suspension, respectively. In the preceding equations, i F ( i = 1, 2, 3, 4, 5, 6) acting as the force between the suspension and the vertical body can be calculated as follows: Where i K is stiffness coefficients, i C is damping coefficients, and (t) i f is the force generated by actuators. In order to improve the flexibility of vehicle-suspension control, our vehicle-

Model Predictive Control with Preview
Our autonomous vehicle was equipped with an active suspension system for solving the problem of vehicle stability when driving on complex uneven terrain, which could be sampled as a nine-degree-of-freedom vehicle model, as shown in Figure 6. The vehicle model consisted of six vertical unsprung masses and degrees of freedom due to the pitch, roll, and vertical motion of the mass center.
Sensors 2020, 20, x FOR PEER REVIEW 9 of 17 suspension system only consisted of actuators, as shown in Figure 1. Thus, stiffness coefficient i K and damping coefficient i C were set to zero.
The dynamic equation of the vertical motion of the unsprung mass is as follows: Where mi K and mi C are the stiffness and damping coefficients of the tires, respectively.
According to the spatial motion law of rigid bodies, the dynamic relationship among the six suspension systems and the body centroid can be obtained as: Suppose that the pitch and roll angles are sufficiently small, then the Equation (20) can be rewritten as follows: The Equations (17)(18)(19)(20)(21) can be rewritten into a state-space formulation: State and output vectors can be expressed as follows: According to Newton's second law, the vertical motion of the body centroid, the pitching rotation, and the roll rotation equations can be obtained as: where F i is the force generated by the actuator; Z, the vertical displacement of vehicle centroid; θ, the pitch angle of vehicle centroid; ϕ, the roll angle of vehicle centroid; and J XX and J YY , the moments of inertia of the X and Y axes, respectively. L r and L f are the distance between the center of mass, and front and rear suspension, respectively. L a and L b are the distance between center of mass, and left and right suspension, respectively. In the preceding equations, F i (i = 1, 2, 3, 4, 5, 6) acting as the force between the suspension and the vertical body can be calculated as follows: (18) where K i is stiffness coefficients, C i is damping coefficients, and f i (t) is the force generated by actuators.
In order to improve the flexibility of vehicle-suspension control, our vehicle-suspension system only consisted of actuators, as shown in Figure 1. Thus, stiffness coefficient K i and damping coefficient C i were set to zero. The dynamic equation of the vertical motion of the unsprung mass is as follows: where K mi and C mi are the stiffness and damping coefficients of the tires, respectively. According to the spatial motion law of rigid bodies, the dynamic relationship among the six suspension systems and the body centroid can be obtained as: Suppose that the pitch and roll angles are sufficiently small, then the Equation (20) can be rewritten as follows: sin θ ≈ θ, sin ϕ ≈ ϕ The Equations (17)- (21) can be rewritten into a state-space formulation: where X M (t) and Y M (t) denote state and output vectors, respectively; A M , B M , C M , and D M are system matrices; E W is the road disturbance matrix; and W(t) is the road disturbance, which can be obtained by the estimated elevation map. State and output vectors can be expressed as follows: where the estimated values Z, θ, and ϕ were obtained by IMU/GPS mounted on the vehicle body; and Z Mi − Z mi were obtained from the direct measurement of active suspension actuator displacement. Control methods have been proposed to optimize Equation (23) to improve vehicle stability. There are two factors that needed to be considered as the suspension-control method of our vehicle. First, our vehicle, being heavy-duty, has strict actuator-performance limits to ensure driving safety. For another, the estimated terrain map needs to be used more effectively, rather than just as a reference index for path planning. Model predictive control was chosen as the actuator-control method because it not only considers actuator limitations in the process of vehicle optimization, but can also effectively apply terrain information to improve control effect. Several model predictive controllers for vehicle active suspension systems have been proposed [21,28], but they did not give a detailed description of estimating elevation map. In this paper, we proposed the method for road profile estimation and model predictive control based on reference [27]. What is more, we gave outdoor experiment results rather than simulation analysis.
To design a model prediction controller, Equation (22) requires first-order Taylor expansion: . .
where . (26) needs to be linearized as follows: With Equation (27), the following equation can be obtained: where According to Equation (28), the prediction of system states can be given by: With Equations (24) and (29), the following equation can be obtained: A generic model predictive controller finds the optimal sequence of control inputsÛ to minimize cost function J MPC : minJ MPC = min Ŷ T P 1Ŷ +X T P 2X +Û T P 3Û Û min ≤Û ≤Û max where P 1 , P 2 , and P 3 are weight matrices of which the parameters can be adjusted to achieve different performance improvements for the vehicle, such as passenger ride comfort, handling stability, and driving safety. In this study, our objective is to minimize ..
ϕ to maintain the stability of the vehicle. With Equation (32), the suspension control inputsÛ could be calculated and applied to the suspension control.

Discussion
Three experiments were carried out to demonstrate the effectiveness and feasibility of our method. The first experiment was to confirm that the proposed MPC method improved the stability of the vehicle compared with the output-feedback-control (OFC) method and a passive suspension system. OFC is a common suspension-control method that uses IMU data as a reference index to adjust the active suspension system. The second experiment was to verify that our method for road profile estimation reduces the drift of LiDAR data, thus obtaining a more accurate and stable elevation map. The third experiment was carried out on continuous uneven terrain to verify the stability of our method for a long period.
Experiments were carried out on the test site with some obstacles, as shown in Figure 7. To carry out the first experiment, we made our vehicle directly pass an obstacle using a passive suspension system, MPC method, and OFC method, respectively. The attitude angle and position information obtained by IMU/GPS is shown in Figure 8. Both the MPC and OFC methods reduced the attitude-angle and vertical-displacement values compared with the passive suspension mode. The root-mean-square (RMS) values of displacement and angle for the vertical, pitch, and roll motions are given in Table 1. RMS values of Z, θ, and ϕ for the OFC method decrease by 37%, 41%, and 46%, whereas reductions for the MPC were 71%, 70%, and 63% when the vehicle was running at a speed of 35 km/h. These results confirmed the efficiency of the proposed MPC method. Other laboratory data with the vehicle running at a speed of 45 km/h are also shown Table 1. As the results demonstrated, the effectiveness of the two control methods was reduced when the speed of the vehicle increased. However, the MPC method still had a good performance in terms of vehicle stability because it calculated the road input in advance on the basis of the estimated elevation map, which ensured that the input value of the actuators could be calculated in advance.
Sensors 2020, 20, x FOR PEER REVIEW 12 of 17 effectiveness of the two control methods was reduced when the speed of the vehicle increased. However, the MPC method still had a good performance in terms of vehicle stability because it calculated the road input in advance on the basis of the estimated elevation map, which ensured that the input value of the actuators could be calculated in advance.     1.61 0.61 (−31%) 0.64 (−60%) In order to verify the accuracy of our method for road profile estimation, we made the vehicle pass two obstacles at the same speed using the active and passive suspension system, respectively. The attitude angle and position information obtained by IMU/GPS is shown in Figure 9. Results demonstrated the improvement of vehicle stability using MPC methods, which is the same as the conclusion of previous experiment results. It is difficult to display details of the elevation terrain since cloud data points obtained by LiDAR are very dense, so we imported LiDAR data into MATLAB by  In order to verify the accuracy of our method for road profile estimation, we made the vehicle pass two obstacles at the same speed using the active and passive suspension system, respectively. The attitude angle and position information obtained by IMU/GPS is shown in Figure 9. Results demonstrated the improvement of vehicle stability using MPC methods, which is the same as the conclusion of previous experiment results. It is difficult to display details of the elevation terrain since cloud data points obtained by LiDAR are very dense, so we imported LiDAR data into MATLAB by means of interpolation technology to show more details. Figure 10a shows the road profile when the vehicle was passing through obstacles in passive suspension mode. When the front wheels of the vehicle passed the obstacle, the vibration of the vehicle body caused drifts of LiDAR data, which caused a large error in the local elevation map (x = 300-450 cm). Details of the local elevation map (x = 300-450, y = 200 cm) are displayed in the local view of Figure 10a. Blue and red lines represent the upper and lower bounds of the confidence interval, respectively, and the green line represents the estimated elevation value. It has been seen that the maximal error of the estimated elevation value was~5.68 cm, and the confidence interval was not guaranteed to contain the true elevation values. Figure 10b shows the road profile when the vehicle was passing through obstacles in active suspension mode. Results showed that LiDAR drifts were effectively suppressed due to the improvement of vehicle stability. The local view also shows that the maximal error of the estimated elevation value was 2.20 cm, and the true elevation values were in the confidence interval. These findings further confirm the efficiency and accuracy of the proposed method for road-profile estimation. was ~5.68 cm, and the confidence interval was not guaranteed to contain the true elevation values. Figure 10b shows the road profile when the vehicle was passing through obstacles in active suspension mode. Results showed that LiDAR drifts were effectively suppressed due to the improvement of vehicle stability. The local view also shows that the maximal error of the estimated elevation value was ~2.20 cm, and the true elevation values were in the confidence interval. These findings further confirm the efficiency and accuracy of the proposed method for road-profile estimation.   To demonstrate the stability of our method for continuous uneven terrain, we made the vehicle pass through a row of obstacles in the active and passive suspension modes, respectively. Attitude angle and position information is shown in Figure 11. Results illustrated that our proposed method still had good stability performance in the suppression of vehicle vibration in the case of continuous road disturbances. Figure 12a shows the local elevation map (x = 0-850, y = 200 cm) when the vehicle was using the active suspension mode. The estimated elevation value approximated the real terrain value, with the maximal error of the estimated elevation value being about 2.15 cm. Figure 12b shows estimated elevation information when the vehicle was using the passive suspension mode. It has been seen that the instability of the vehicle body was aggravated in a continuous-uneven-terrain environment, resulting in more serious drifts of LiDAR data. Furthermore, the Mahalanobis distance criterion also lost its effect in certain positions, which results in cloud data points in the same cell not being able to be effectively fused. To demonstrate the stability of our method for continuous uneven terrain, we made the vehicle pass through a row of obstacles in the active and passive suspension modes, respectively. Attitude angle and position information is shown in Figure 11. Results illustrated that our proposed method still had good stability performance in the suppression of vehicle vibration in the case of continuous road disturbances. Figure 12a shows the local elevation map (x = 0-850, y = 200 cm) when the vehicle was using the active suspension mode. The estimated elevation value approximated the real terrain value, with the maximal error of the estimated elevation value being about 2.15 cm. Figure 12b shows estimated elevation information when the vehicle was using the passive suspension mode. It has been seen that the instability of the vehicle body was aggravated in a continuous-uneven-terrain environment, resulting in more serious drifts of LiDAR data. Furthermore, the Mahalanobis distance criterion also lost its effect in certain positions, which results in cloud data points in the same cell not being able to be effectively fused.

Conclusions
This paper proposed a novel method for road profile estimation using LiDAR, IMU/GPS, and a vehicle with an active suspension system. The first was the method to obtain the probability elevation terrain using GPS/INS and 3D laser scanners. The second was the description of a model predictive control method, which fused the IMU/GPS and obtained an elevation map to adjust the vehicle suspension system to maintain the stability of the vehicle. Real experiment results demonstrated that the safety and stability of the vehicle driving on complex uneven terrain could be ensured. The elevation information estimation using our method was also more accurate and stable because it avoided the drifts of LiDAR data. There were also some limitations in the whole study. For example, GPS data is unstable under certain conditions, which led to an imprecise location. Further, the model predictive control method needed a higher computational load compared with other control methods, which limited its application in engineering practice. In the future, we will consider how to further improve the accuracy of elevation maps and vehicle stability on complex roads.