Research on Lane a Compensation Method Based on Multi-Sensor Fusion

The curvature of the lane output by the vision sensor caused by shadows, changes in lighting and line breaking jumps over in a period of time, which leads to serious problems for unmanned driving control. It is particularly important to predict or compensate the real lane in real-time during sensor jumps. This paper presents a lane compensation method based on multi-sensor fusion of global positioning system (GPS), inertial measurement unit (IMU) and vision sensors. In order to compensate the lane, the cubic polynomial function of the longitudinal distance is selected as the lane model. In this method, a Kalman filter is used to estimate vehicle velocity and yaw angle by GPS and IMU measurements, and a vehicle kinematics model is established to describe vehicle motion. It uses the geometric relationship between vehicle and relative lane motion at the current moment to solve the coefficient of the lane polynomial at the next moment. The simulation and vehicle test results show that the prediction information can compensate for the failure of the vision sensor, and has good real-time, robustness and accuracy.


Introduction
In recent years, intelligent driving vehicles have received widespread attention. The reason is that intelligent driving vehicles can play a positive role in the daily traffic environment. The reliability of intelligent driving control system with mature technology is higher than that of drivers with different driving skills. Important functions of intelligent driving, such as lane keeping systems (LKAs) and lane change systems (LCXs) have been widely studied. In order to ensure autonomous vehicles can drive safely on the road, high-precision lane level location is required. Currently, lane-level location can be realized through Lidar, GPS/INS or cameras. However, the cost of Lidar is higher than that of other sensors. When GPS and INS are used for high-precision positioning, the absolute position information is obtained, which must be matched with a high-precision map to obtain the relative road position information. Therefore, high-efficiency and low-cost environmental perception based on vision will become the main direction of future industrialization of intelligent driving vehicles [1].
Lane recognition [2,3] is an important part of lane-level location. At present, the mainstream method is to obtain lane images through vision sensors installed on intelligent vehicles, and then use the edge, color, texture or frequency domain features of lanes to separate lane lines from road areas. According to the different image processing strategies of road conditions, existing methods can generally be divided into model-based methods [4][5][6][7][8] and feature-based methods [9][10][11][12][13]. However, the acquisition of lane information from vision sensors may fail due to complex shadows, missing lane

Yaw Angle and Longitudinal Velocity Estimation
This section focuses on estimation of the vehicle trajectory using an IMU and a GPS receiver. The vehicle velocity and yaw angle can be estimated by a sensor fusion algorithm using the data obtained from an IMU and a GPS. The main source of inertial sensor error is drift caused by sensor deviation and gravity effects. Therefore, the method in this paper aims to model these error sources and ignore the influence of cross-coupling errors and sensor scale factor errors. The IMU offers six degrees of freedom and consists of a three-axis gyroscope and a three-axis accelerometer which are installed on the carrier. In this paper, only the z-axis of the gyro and x-axis of the accelerometer are modeled as required.

Gyro Modeling
The output of the gyroscope can be expressed as the real vehicle yaw rate, with zero deviation and white noise, as shown in Equation (1): where, g r is the output of the gyro, r is the yaw rate of vehicle, b r is the constant offset or bias of the gyro, ω gyro is the zero mean white noise of sensor. Assuming that the sensor noise obeys normally distributed, and the sampled covariance is E ω 2 gyro = σ 2 gyro . The deviation of the gyroscope is represented by a first order Markov process: where, T g is the correlation time. ω g_bias is the process driving noise of the gyro which is normally distributed with zero mean and a sampled covariance of E ω 2 g_bias = σ 2 b g .

Accelerometer Modeling
The accelerometer is modeled in the same way as the gyro. The influence of Coriolis acceleration and gravity acceleration were not considered in the modeling process: where, a is the output of accelerometer, b a is the constant offset or bias of the accelerometer. ω accel is the process driving noise of the accelerometer which is obeying normally distributed and the sampled covariance is E ω 2 accel = σ 2 accel . The accelerometer bias is also represented by a first order Markov process: where, T a is the correlation time. ω a_bias is the process driving noise of the accelerometer which is obeying normally distributed and the sampled covariance is E ω 2 a_bias = σ 2 b a .

Kalman Filter Establishment
Velocity, gyro bias, yaw angle and accelerometer zero bias were selected as state variables which are presented as The longitudinal acceleration and yaw rate are the measurements of IMU, expressed as u = g r a T . According to Equations (1) and (4), the equation of state of the system is: x = Ax + Bu + ω (5) where: The velocity and yaw angle measured by GPS are selected as external observation parameters and the system measurement equation is established as follows: where, µ is the sensor noise, represented as The covariance matrices Q and R of the process noise and the measured noise are solved, and the linear Kalman filter is used for the optimal estimation. Readers may refer to [41] for details.

Vehicle Kinematics
The sensor fusion information is used to build the vehicle kinematics model, so as to obtain the vehicle trajectory. Firstly, the vehicle motion is simplified and described as a motion in a two-dimensional plane, as shown in Figure 1. The kinematics model uses three parameters to describe the motion of the vehicle, which represents the current abscissa X(t) of the vehicle, the current ordinate Y(t) of the vehicle and the vehicle's current yaw angle ψ(t). The global coordinate system and the local coordinate system are respectively established at the center of mass of the vehicle. The horizontal axis of the global coordinate system is X and the vertical axis is Y. The horizontal axis of the local coordinate system is x and the vertical axis is y. In Figure 1, x 0 and y 0 are the local coordinate systems established by the vehicle at time t 0 . ψ is the angle between the longitudinal axis of the vehicle and the X-axis. V is the velocity at the center of mass of the vehicle. V X and V Y are the projection of the vehicle velocity on the X-axis and Y-axis in the global coordinate system. The kinematics equation of the vehicle in the global coordinate system is: . . .
From Equations (11)- (13), it can be seen that the vehicle's motion position is determined by the yaw rate, longitudinal acceleration of the center of mass and sideslip angle. Sensors 2018, 18, x FOR PEER REVIEW 5 of 17 Figure 1. Vehicle kinematics model.
In the global coordinate system, the vehicle position is described as [ ] X Y ψ . Assuming that the vehicle's position at initial time 0 , then the vehicle's position at time 1 t can be expressed as follows:

Vehicle Trajectory and Lane Polynomial
The vision sensor performs lane detection in a local coordinate system. Therefore, the vehicle trajectory and lane are defined in the local coordinate system of the vehicle. Lane curves and vehicle motion trajectories are shown in Figure 2.  In the global coordinate system, the vehicle position is described as X Y ψ . Assuming that the vehicle's position at initial time t 0 is P( T , then the vehicle's position at time t 1 can be expressed as follows:

Vehicle Trajectory and Lane Polynomial
The vision sensor performs lane detection in a local coordinate system. Therefore, the vehicle trajectory and lane are defined in the local coordinate system of the vehicle. Lane curves and vehicle motion trajectories are shown in Figure 2.  Figure 1. Vehicle kinematics model.
In the global coordinate system, the vehicle position is described as [ ] X Y ψ . Assuming that the vehicle's position at initial time 0 , then the vehicle's position at time 1 t can be expressed as follows:

Vehicle Trajectory and Lane Polynomial
The vision sensor performs lane detection in a local coordinate system. Therefore, the vehicle trajectory and lane are defined in the local coordinate system of the vehicle. Lane curves and vehicle motion trajectories are shown in Figure 2.  Assuming that the vehicle is moving at constant velocity V x and yaw rate . ψ, the vehicle trajectory can be approximated by a parabolic projection. The vehicle trajectory f v (x) is expressed as follows: At present, the general method is to describe the two-dimensional geometry of the lane through the cyclotron lines model. Taking an expressway as an example, the radius of lanes is generally more than 100 m, so the curvature and curvature rate are usually small and the geometry of the lane can be expressed by a cubic polynomial equation. Through the cubic polynomial, the left and right lanes can be described as follows: where, c L0 and c

Lane Parameters Estimation
The system is a multi-rate system. The visual sensor, IMU, GPS and vehicle controller operate at different update rates, and the data can be obtained through the vehicle CAN bus. The information transmission of sensors and controllers is shown in Figure 3. At present, the general method is to describe the two-dimensional geometry of the lane through the cyclotron lines model. Taking an expressway as an example, the radius of lanes is generally more than 100 m, so the curvature and curvature rate are usually small and the geometry of the lane can be expressed by a cubic polynomial equation. Through the cubic polynomial, the left and right lanes can be described as follows: where, 0 where,

Lane Parameters Estimation
The system is a multi-rate system. The visual sensor, IMU, GPS and vehicle controller operate at different update rates, and the data can be obtained through the vehicle CAN bus. The information transmission of sensors and controllers is shown in Figure 3. Figure 3. System block diagram.
In Figure 3, the update rate of vision sensor is cam  Figure 4, the time constant t is defined as: In Figure 3, the update rate of vision sensor is T cam , the update rate of IMU is T c , and the update rate of GPS is T G . The relationship between the three update rate is T G > T cam > T c . Since the Kalman filter and IMU operate at the same rate, we only focus on the relationship between T cam and T c . Assuming that there is an integer n(n > 1) that makes T cam = n · T c , according to the difference of update rate between IMU and the camera sensor which shown in Figure 4, the time constant t is defined as: where, k(k = 0, 1, · · ·) and i(i = 0, 1, · · · n − 1) represent the update periods of the vision sensor and IMU, respectively. It is assumed that the vision sensor at time k can obtain stable lane information, and the longitudinal velocity and yaw angle can be obtained after the fusion of IMU and GPS data. Based on the difference in the sensor update rate, the vehicle trajectory can be continuously calculated within the interval of the vision sensor update. Assuming that the initial vehicle position at time k is where V x is the component of the speed V in the local coordinate system of the vehicle. Vehicle position and trajectory equation are solved by using Equations (15) and (20)- (22). The prediction method is derived below to predict the coefficient of lane polynomial at time k + 1.
where x V is the component of the speed V in the local coordinate system of the vehicle.
Vehicle position and trajectory equation are solved by using equations (15) and (20)- (22). The prediction method is derived below to predict the coefficient of lane polynomial at time 1 k + .

Lateral Offset Estimation
In the local coordinate system, the relationship between the vehicle center of mass and the lane is used to predict the lane coefficient, which is shown in Figure 5.

Lateral Offset Estimation
In the local coordinate system, the relationship between the vehicle center of mass and the lane is used to predict the lane coefficient, which is shown in Figure 5.
where x V is the component of the speed V in the local coordinate system of the vehicle.
Vehicle position and trajectory equation are solved by using equations (15) and (20)- (22). The prediction method is derived below to predict the coefficient of lane polynomial at time 1 k + .

Lateral Offset Estimation
In the local coordinate system, the relationship between the vehicle center of mass and the lane is used to predict the lane coefficient, which is shown in Figure 5.
In the plane rectangular coordinate system, the angle between g(x) and the x-axis is π 2 +ψ(k + 1), and the slope and intercept are expressed as m and b, respectively: For simultaneous Equations (18) and (23): The horizontal coordinate of point Q k+1 can be solved that using the secant method. Let's define the coordinates Q k+1 x Q (k + 1|0), f L x Q (k + 1|0) . Then, the Euclidean distance between P k+1 and Q k+1 can be obtained, namely, the lateral offsetĉ 0 (k + 1|0): where:

Curvature and Curvature Rate Estimation
According to the design characteristics of expressway, it is approximately considered that the curvature rate of lane is constant in a single period. According to Equation (18), f L (x) = 2c 2 + 6c 3 · x, the vehicle position at the current moment is zero in the local coordinate system of the vehicle, so f L (0) = 2c 2 . Then we will calculate the curvature and curvature rate at time k + 1: where, κ is an tuning parameter related to vehicle speed, and the value is approximately equal to the sampling time of the vision sensor.
If the sensor fails to provide lane information within several sampling cycles, the predictive compensation method derived above is used for road compensation. However, when the sensor is ineffective for a long time, the performance of lane prediction will decline due to the lack of road information feedback.

Experimental Results
The method is evaluated by hardware-in-the-loop simulation platform and vehicle experiments.

Simulation
In the hardware-in-the-loop simulation, Carsim software is used to simulate a A-class vehicle. In the simulation process, the velocity is set to 25 m/s (i.e., 90 km/h). The expected path is a third-order polynomial curve that conforms to cyclotron constraints. The parameters used in Carsim are the nominal values of the test vehicle. The HIL platform is shown in Figure 6, and the HIL process is shown in Figure 7. where, κ is an tuning parameter related to vehicle speed, and the value is approximately equal to the sampling time of the vision sensor.
If the sensor fails to provide lane information within several sampling cycles, the predictive compensation method derived above is used for road compensation. However, when the sensor is ineffective for a long time, the performance of lane prediction will decline due to the lack of road information feedback.

Experimental Results
The method is evaluated by hardware-in-the-loop simulation platform and vehicle experiments.

Simulation
In the hardware-in-the-loop simulation, Carsim software is used to simulate a A-class vehicle. In the simulation process, the velocity is set to 25 m/s (i.e., 90 km/h). The expected path is a third-order polynomial curve that conforms to cyclotron constraints. The parameters used in Carsim are the nominal values of the test vehicle. The HIL platform is shown in Figure 6, and the HIL process is shown in Figure 7.

Angle senor signal
Pinion angle  where, is an tuning parameter related to vehicle speed, and the value is approximately equal to the sampling time of the vision sensor.

27-DOF Nonlinear Vehicle Model
If the sensor fails to provide lane information within several sampling cycles, the predictive compensation method derived above is used for road compensation. However, when the sensor is ineffective for a long time, the performance of lane prediction will decline due to the lack of road information feedback.

Experimental Results
The method is evaluated by hardware-in-the-loop simulation platform and vehicle experiments.

Simulation
In the hardware-in-the-loop simulation, Carsim software is used to simulate a A-class vehicle. In the simulation process, the velocity is set to 25 m/s (i.e., 90 km/h). The expected path is a third-order polynomial curve that conforms to cyclotron constraints. The parameters used in Carsim are the nominal values of the test vehicle. The HIL platform is shown in Figure 6, and the HIL process is shown in Figure 7.

Angle senor signal
Pinion angle  As shown in Figure 7, the 27-DOF nonlinear vehicle model of Carsim is used to simulate the virtual vehicle. The state input of vehicle, feedback of steering controller and compensation algorithm all run in AutoBox which from dSPACE. The communication between each part is realized by using CAN bus.

27-DOF Nonlinear Vehicle Model
The prediction performance of the method is verified based on the HIL platform, and the vehicle steering change process caused by sensor failure during lane restore is simulated. In the process of simulation, the following three scenarios were considered: (a) The vision sensor can detect the road information normally; (b) the vision sensor fails every 10 s, and the failure time is variable; (c) when the vision sensor fails, the compensation algorithm is used to restore the lane. algorithm all run in AutoBox which from dSPACE. The communication between each part is realized by using CAN bus.
The prediction performance of the method is verified based on the HIL platform, and the vehicle steering change process caused by sensor failure during lane restore is simulated. In the process of simulation, the following three scenarios were considered: (a) The vision sensor can detect the road information normally; (b) the vision sensor fails every 10 s, and the failure time is variable; (c) when the vision sensor fails, the compensation algorithm is used to restore the lane. Figure 8 shows the comparison of lateral offset, heading angle, curvature and curvature rate in the three scenarios. The blue line shows the data measured by the virtual sensor. The red line shows the measured data which contains the failure of the virtual sensor. The green line shows the data predicted using the compensation algorithm. On the road, the failure occurred at 5, 15, 25, 35, 45 and 55 s, respectively. The first 50 s shown in the figure are a variable curvature road and the remaining time is an approximately straight road. The results show that the method can achieve good results on both straight and curved roads.   Table 1. The lateral displacement of the error is less than ± 2 × 10 -2 m, heading angle error is less than ± 4 × 10 -4 rad, curvature of the error is less than ± 1.2 × 10 -5 m -1 , the error of the curvature change rate is less than 1.5 × 10 -7 m -2 . The RMSE of each variable is far less than the magnitude of the error. This indicates that the predicted lane is very close to the compensated lane. The steering angle error caused by the maximum prediction error is far less than 1°, so the influence of the prediction error on the control accuracy can be neglected.    Table 1. The lateral displacement of the error is less than ±2 × 10 −2 m, heading angle error is less than ±4 × 10 −4 rad, curvature of the error is less than ±1.2 × 10 −5 m −1 , the error of the curvature change rate is less than 1.5 × 10 −7 m −2 . The RMSE of each variable is far less than the magnitude of the error. This indicates that the predicted lane is very close to the compensated lane. The steering angle error caused by the maximum prediction error is far less than 1 • , so the influence of the prediction error on the control accuracy can be neglected.  Table 1. The lateral displacement of the error is less than ± 2 × 10 -2 m, heading angle error is less than ± 4 × 10 -4 rad, curvature of the error is less than ± 1.2 × 10 -5 m -1 , the error of the curvature change rate is less than 1.5 × 10 -7 m -2 . The RMSE of each variable is far less than the magnitude of the error. This indicates that the predicted lane is very close to the compensated lane. The steering angle error caused by the maximum prediction error is far less than 1°, so the influence of the prediction error on the control accuracy can be neglected.    Considering the safety, HIL is used to verify the lane-keeping based on compensation method. We design a lane keeping controller based on PID theory. In the program, bad points are set manually. In Figure 10, the blue line indicates lane-keeping control with sudden bad points, while the red dotted line represents lane-keeping control after compensation by the algorithm and the tracking effect is represented by steering wheel angle. At 25 s, due to the appearance of bad points, the uncompensated control produces the sudden change of steering wheel angle, which leads to the instability of the controlled vehicle and easily causes accidents. After compensation, the control effect is stable, and the vehicle smoothly transits to the sensor and returns to normal. Considering the safety, HIL is used to verify the lane-keeping based on compensation method. We design a lane keeping controller based on PID theory. In the program, bad points are set manually. In Figure 10, the blue line indicates lane-keeping control with sudden bad points, while the red dotted line represents lane-keeping control after compensation by the algorithm and the tracking effect is represented by steering wheel angle. At 25 s, due to the appearance of bad points, the uncompensated control produces the sudden change of steering wheel angle, which leads to the instability of the controlled vehicle and easily causes accidents. After compensation, the control effect is stable, and the vehicle smoothly transits to the sensor and returns to normal.  Figure 11 shows the experimental vehicle and the experimental road section. The experimental vehicle is equipped with sensors such as Mobileye, IMU and GPS. The update rate of each sensor is shown in Table 2. The update periods of ECU controller, visual sensor, IMU and GPS are all different. This is a typical multi-rate system. The ECU controller communicates with the sensors through the CAN bus. The vehicle velocity is controlled between 60 km/h and 70 km/h. The prediction method is compared with the measured data of the vision sensor. The characteristics of the experimental road are as follows:   Figure 11 shows the experimental vehicle and the experimental road section. The experimental vehicle is equipped with sensors such as Mobileye, IMU and GPS. The update rate of each sensor is shown in Table 2. The update periods of ECU controller, visual sensor, IMU and GPS are all different. This is a typical multi-rate system. The ECU controller communicates with the sensors through the CAN bus. The vehicle velocity is controlled between 60 km/h and 70 km/h. The prediction method is compared with the measured data of the vision sensor. The characteristics of the experimental road are as follows:   Figure 12 shows the performance of the estimation method in a vehicle test. The red line shows the measured data by the vision sensor. The blue dots line shows the estimated data by the compensation algorithm. In the test process, when one of the two lines cannot be detected, it is also considered a failure. It can be seen that the compensation effect is good in straight and curve sections.

Vehicle Test
vehicle is equipped with sensors such as Mobileye, IMU and GPS. The update rate of each sensor is shown in Table 2. The update periods of ECU controller, visual sensor, IMU and GPS are all different. This is a typical multi-rate system. The ECU controller communicates with the sensors through the CAN bus. The vehicle velocity is controlled between 60 km/h and 70 km/h. The prediction method is compared with the measured data of the vision sensor. The characteristics of the experimental road are as follows:  Figure 11. Experimental vehicles and test sites. Figure 11. Experimental vehicles and test sites.   Figure 12 shows the performance of the estimation method in a vehicle test. The red line shows the measured data by the vision sensor. The blue dots line shows the estimated data by the compensation algorithm. In the test process, when one of the two lines cannot be detected, it is also considered a failure. It can be seen that the compensation effect is good in straight and curve sections.        (c) k+4 (d) k+6 Figure 13. Lane estimation at time k, k + 2, k + 4, k + 6. Figure 13. Lane estimation at time k, k + 2, k + 4, k + 6.

Conclusions
In this study, a lane compensation method based on sensor fusion is proposed to compensate for the short-time failure of vision sensors. The method is divided into two parts. The first part is the vehicle trajectory acquisition based on sensor fusion, and the second part is the lane prediction using the relative position of the vehicle and road. The hardware-in-the-loop and vehicle experiments show that the short-time failure of vision sensors can be compensated by this method.
The algorithm has two main contributions. Firstly, a lane compensation method based on sensor fusion is proposed. That is, through the fusion of low-cost sensors, high-precision vehicle status information can be obtained, and then the relative position of vehicles and roads can be used to predict the road coefficient. Secondly, the simulation and vehicle experiments are used to verify the effectiveness and real-time performance of the lane compensation algorithm. It validates the applicability of the algorithm in the case of discontinuous bad points and short duration of bad points.
In the future, we will use more complex working conditions to verify the algorithm and consider combining other sensors to extend the compensation time. Based on this, we will consider the impact of speed on the algorithm to improve the compensation method. In addition, lane detection method will also be focus of our research.