Dual-rate lane-keeping control for an autonomous ground vehicle. Comparison of different strategies

In this contribution, different lane-keeping control strategies for Autonomous Ground Vehicles (AGV) have been analyzed and compared. The AGV must be oriented and kept within a given reference path using the front wheel steering angle as the control action for a specific longitudinal velocity. While non-linear models can describe the lateral dynamics of the vehicle in an accurate manner, they might lead to difficulties when computing some real-time control laws such as Model Predictive Control (MPC). Linear Parameter Varying (LPV) models can provide a trade-off between computational complexity and model accuracy. Another way to reduce computational complexity is to explore other control strategies, for example, the one based on the Inverse Kinematic Bicycle model (IKIBI). Additionally, AGV sensors typically work at different measurement acquisition frequencies so that Kalman Filters (KF) are usually needed for sensor fusion. If these frequencies are slower than the actuation rate, a multi-rate KF may be needed. The two control strategies (MPC using a LPV model and IKIBI) have been compared in simulations over a circuit path in the presence of process and measurement Gaussian noise. The MPC controller has shown to provide a more accurate lane-keeping behavior than an IKIBI control strategy. Finally, it has been seen that Dual-Rate Extended Kalman Filters (DREKF) constitute an essential tool when only slow and noisy sensor feedback is available in an AGV lane-keeping application.


Introduction
Self-driving cars have been increasing their popularity year after year. They are the type of Autonomous Ground Vehicles (AGV) that receive the greatest share of attention, both in academia and in industry, because of the possibility that they shift the paradigm of transportation systems. An essential concern in the development of these automated driving systems is the ability to obtain a controller that is able to make the vehicle follow a pre-established path. This problem is often considered, in a hierarchical manner, as the low level control of the AGV in opposition to a high level control, which is focused on path or trajectory generation based on the awareness of the environment that surrounds the vehicle.
The lateral vehicle control takes care of the path-tracking problem. The path is composed by a sequence of positions and orientations in the plane, and the controller has to make sure that the vehicle follows them. In order to control the vehicle, two input variables are often considered: the steering angle (which is modified by acting on the steering wheel) and the longitudinal acceleration (modified via throttle). If the vehicle must follow a feasible collision-free pre-computed path with no time constraints, the problem is known as lane keeping. On the contrary, if each pair of positions and orientations of the pre-defined path has a time stamp associated to them, the problem will be considered as a trajectory-tracking problem. Since time constraints will not be considered in this Moreover, as previously commented, in this control problem some sensors work at a slower rate. In order to reach a good control performance, this rate may not be appropriate to update the controller output. Then, instead of using a DREKF to provide a single, fastrate controller with faster estimates, a dual-rate controller may be considered to generate faster control actions from slower measurements.
The main contributions of this article are three. First, to introduce a dual-rate EKF (DREKF) that allows a fast state update using slow and noisy measurements in a autonomous vehicle control context. Second, to present a LPV-MPC design that considers a model identified specifically for longer time scale predictions such as the ones handled by MPC. Third, to compare and analyze two different low computationa complexity, dual-rate approaches for lateral vehicle controlling. The first solution considers a DREKF together with a single, fast-rate feedforward controller, which is designed from an inverse kinematics bicycle (IKIBI) model. The second proposal uses an MPC controller, which can be designed from a new LPV optimized model, and with a prediction horizon that allows to generate a fast-rate control signal from the slow-rate measurements.
The paper is organized as follows. Section 2 details design aspects for each control approach (IKIBI and MPC). Then, DREKF is introduced in Section 3. Simulated experiments are introduced and justified in Section 4, and their results are presented and discussed in Section 5. Finally, some conclusions summarize the present work in Section 6.

Control strategies
There are diverse control laws devoted to vehicle keeping lane, commonly called as steering controllers. In this section, two widely used methods with some variations will be considered: the inverse kinematic bicycle model (IKIBI) and the Linear Parameter-Varying Model Predictive Control (LPV-MPC).
In both cases, the purpose is to use the steering front wheels angle δ as the control action in order to follow the desired path. The complete path, X Y Ψ traj , is planned offline, and depending on the controller election, the next yaw rate, r re f , or yaw position goal, Ψ re f , will be delivered by a pure pursuit procedure with a coherent look-ahead distance L [7,19,33,34]. Figure 1 shows a schematic view of this process. The Dual-Rate Extended Kalman Filter (DREKF) propose for state estimation can also be seen in Fig. 1, and will be further explained in Section 3.

Control Plant
Look Ahead

Inverse kinematic bicycle model (IKIBI) based controller
In this work, IKIBI is used by adding a proportional feedforward controller in order to consider the yaw rate measurement r. The control law yields where K p is the feedforward controller's proportional gain, r re f is the yaw rate goal established by the pure pursuit, L is the vehicle's longitudinal length, and γ is a vehicle coefficient that translates the tire angle into steering angle. Since the input signal will be considered directly as the tire angle, γ = 1. Also, the function atan 2 represents the fourth-quadrant inverse tangent.

Linear Parameter Varying -Model Predictive Control (LPV-MPC)
Model Predictive Control can be used for lateral vehicle control [8,20,21]. A linear model of the system should be considered to implement an MPC controller in real-time due in order to avoid computational delays [21,35,36]. The lateral dynamics' model that will be used for this controller was presented in [27]: whereψ is the yaw rate in body frame coordinates of the vehicle and δ the front steering angle. The presented model is Linear Parameter Varying (LPV) and its coefficients (b 0−2 and a 1,2 ) will depend on the lateral acceleration and longitudinal velocity in the local body frame of the vehicle. Previous research has suggested an optimized method for identifying the system's parameters [31] by minimizing the model prediction errors over a long-time horizon.
Since the goal of this controller is to follow a trajectory reference described in terms of position (X and Y) and orientation (Ψ) in absolute coordinates, it is interesting to set the orientation of the vehicle (ψ) as the output of the system rather that its rate of change (ψ). A forward Euler method has been used where: where T is the sampling period of the system. Then: Therefore, the model being used in MPC will be: where u and y are the discrete-time input (δ(k)) and output (ψ(k)) variables, respectively, and: The quadratic cost function chosen for solving this problem at each time step k is: where U is the input signal sequence of the control horizon that minimizes the cost function over the MPC prediction horizon at every metaperiod, and Q and R are positive semidefinite weight matrices that penalize the controlled variables and inputs, respectively. Also, y re f (k) will be determined by the Look-Ahead algorithm.
This optimization problem is subject to the discrete-time model of the system (5), ( 6), and a set of linear constraints on the control and the output that preserve the physical feasibility of the solution: where Y is the sequence of discrete-time output variables in the prediction horizon of the MPC problem. Also, the rate of change of the output variable will be limited by the Slew Rate, S, to avoid abrupt vehicle turns that would have a detrimental effect on the passengers comfort: The choice of the cost function as convex, as well as a linear model, and convex constraint sets, makes the whole problem convex, which is beneficial for the computation of the problem since if a solution exists, it is the globally optimal [37].
Finally, one of the aspects of an MPC control is the generation of the sequence of the M future discrete-time control actions to achieve the goal reference. However, it is usual that only the first control action is injected. Therefore, MPC is a natural dual-rate control in the sense that calculates M future control actions with each measurement data.

Dual-rate extended Kalman filter (DREKF)
In previous work [31], the set of hardware available (an Inertial Measurement Unit, a Differential GPS, and a computer) for data acquisition in the car was able to measure X, Y, Ψ, and V x , but V y and r where difficult to access.
Moreover, the measurements are available with different frequencies being the GPS's at a slow frequency (about 10 Hz) and the same for velocity acquisition. The orientation ψ is acquired by IMU with a frequency of 100 Hz. For this reason, that dual-rate control is a natural proposal to deal with this problem, assuming slow-rate measurements but a fast (M times faster) steering control action. The acceleration could be varied with slow frequency. In the case of dual-rate EKF, it may be needed when some of the measurements are not available due to its slow-rate acquisition (for instance, X and Y from the GPS). Then, state estimation is carried out at a faster rate from (V x , ψ).
Analogously, the DREKF includes a linearization procedure, which is based on the use of the Jacobian matrix (a matrix of partial derivatives). At each time step, this matrix is evaluated with the current predicted states. DREKF, different from a standard EKF, carries out some slow-rate computations (such as the correction stage) only when output variables are available, that is, when they are sensed. Otherwise, predictions are shifted to the next iteration.
The DREKF presented in this section takes a non-linear model based on second Newton's law that uses the bicycle model and assumes a constant tire load [5,24,29] for state estimation. Expressing the model in discrete time at period T yields where the different constants and variables were defined in Appendix A. Let us denote this global non-linear dynamic model as the next state-space representation , ψ(k)) T , and n 1 (k − 1) and n 2 (k) are process and measurement noises, respectively, which are both assumed to be zero mean multivariate Gaussian noises with variancē Q(k) = 0.01 andR(k) = 0.01, respectively. Assuming that the notationξ(j|i) means the state estimated for the instant jT at the instant iT, the prediction and correction steps of the DREKF are defined as follows: • Fast-rate calculations: -Prediction of the next stateξ(k|k − 1), and propagation of the covariance P(k|k − 1). These computations are calculated ∀k: the expectation, and where A(k) and L(k) are Jacobian matrices computed in order to respectively linearize the process model about the current state and about the process noiseĀ Preprints (www.preprints.org) | NOT PEER-REVIEWED | Posted: 4 January 2021 doi:10.20944/preprints202101.0015.v1 -State and covariance shifts, forξ(k|k) and P(k|k), respectively. These computations are calculated when measurements are not provided, that is, for k = MT: • Slow-rate calculations, which are computed when measurements are provided, that is, for k = MT: -Prediction of the future outputẑ(k): -Computation of the Kalman filter gain K(k): where H(k) and M(k) are Jacobian matrices calculated in order to respectively linearize the output model about the predicted next state and about the measurement noise -Correction of the stateξ(k|k), and correction of the covariance P(k|k): Finally, it should be mentioned that if M = 1 is assumed, state and covariance shifts in (29) are replaced by corrections in (33), resulting in a (single-rate) EKF.

Implementation
In this section, we present the experiments that have been performed in order to compare the two proposed controllers for lane keeping and justify the appeal of using a DREKF in this application.
The design choices for the controllers and some simulation details are presented first, followed by a discussion of the tests' selection. Afterwards, we introduce the cost indexes that quantify each controllers' performance and we present the results obtained.

Simulation details and design choices for the controllers
Simulations have been carried out using the vehicle parameters of a 2017 Lincoln MKZ on a circuit path. The sampling period of the simulated discrete-time plant is assumed to be T = 0.01 s, which is the same as the fastest acquisition frequency of sensors installed in the test-bed vehicle.
The IKIBI-based controller design results in K p = 0.55 and, as commented earlier, γ = 1. On the other hand, the LPV model parameters used for the MPC strategy were obtained in previous research results [31]. Moreover, the convex optimization problem of the MPC is solved using CVXGEN [38].
Moreover, the prediction and the control horizons in the MPC problem have been chosen to be equal to 10 steps (N = 10), to ensure a small computation time, and the weighting matrices that penalize the output deviation from its reference and the input are, Q = 1 and R = 0.001, respectively.

Performed tests selection
The performed tests compare the behavior of the two proposed controllers, the IKIBIbased controller and MPC-based controller. The tests are performed focusing on the lateral dynamics of the vehicle and therefore, it is assumed that a longitudinal controller is able to maintain a constant longitudinal speed throughout the entire trajectory (a x = 0). The tests have been performed using two different longitudinal velocities: 8 and 12 m/s.
The circuit that has been used to generate the path references includes abrupt lateral movements such as a fast double lane turn and a 180 degrees turn. These maneuvers are so aggressive that when driving the real car through this path, the longitudinal velocity was a low as 2.5 m/s in the most critical segments. Therefore, using constant longitudinal velocities of 8 and 12 m/s will allow us to drive close to the vehicle's dynamic limits.
Moreover, the most realistic tests will assume that new sensor data will be obtained every 0.1 seconds, so M = 10. Thus, only in the presence of the DREKF, both controllers will be able to receive new data every T = 0.01 s. However, if a single rate EKF (SREKF) is implemented instead of the DREKF, the controllers will have to be calculated every MT = 0.1 s.
In this last situation, the MPC-based controller is still able to provide a different control signal every T since M ≤ N, which means that the first M discrete-time control signals (M = 10) of the control horizon (N = 10) will be used at every controller calculation. On the contrary, the IKIBI-based controller has to calculate one control signal every MT in the absence of the DREKF. Finally, both controllers are tested considering process and measurement noises and also without these noises.

Cost indexes used to measure performance
Two different cost indexes will be used in order to better quantify and compare each control solution in each of the tests: • J 1 , which is based on the 2 -norm, and its goal is to provide a measure about how accurately the path is followed: where l is the number of iterations required by the AGV to reach the final point of the path, (X, Y) k is the current AGV position, and (X re f , Y re f ) k is the nearest kinematic position reference to the current AGV position. • J 2 , which is based on the ∞ -norm, and is defined to obtain the maximum difference between the desired path and the current AGV position:

Results and Discussion
This section shows and discusses the results that have been obtained from the different tests.

Noiseless, fast sensor feedback test
The first experiment considers the situation where sensor feedback is received every T (fast sampling rate). Therefore, the controllers can also directly calculate the input signal (steering angle) every T. Moreover, since it is assumed in this test that there is no measurement or process noises, there is no need for a filter.
This test will be used to compare each of the two controllers that we have proposed for this application. Figures 2, 3, and 4 show the results. Figure 2 plots the X and Y coordinates of each simulation, and Figs. 3 and 4, the temporal evolution of the steering angle and the yaw rate, respectively. Because of the abruptness of the maneuvers, it is clearly observed in Fig. 2 a degradation of the behavior when the longitudinal velocity of the vehicle is higher. Such an aggressive maneuver is handled by each of the controllers in two different ways.
On the one hand, the IKIBI controller simply increases the steering angle in order to achieve a higher yaw rate. While this may suffice for more moderate maneuvers, in a real scenario the front wheel angular position is physically bounded, and therefore the control signal calculated with this controller would not be feasible.
The degradation in the lane-following accuracy when the control signal is saturated for the IKIBI controller with the maximum steering angle of the car is can be seen in Fig. 2. Here, the front wheels cannot physically turn more than 0.32 radians. This degradation becomes more noticeable the more aggressive the maneuver is, here, the higher the longitudinal velocity is.
On the other hand, MPC can explicitly consider in its calculations that the front wheel steering angle has to be bounded to never violate the physical limitations of the real vehicle. Moreover, because of the prediction horizon, when the car has to perform an abrupt maneuver, the MPC anticipates to it and starts steering the wheel before the time that the IKIBI controller does.
As commented, Fig. 3 shows the front-wheel steering temporal evolution. It can be seen here how the MPC controller is able to keep the steering angle inside the desired boundaries whereas the IKIBI controller will saturate. Moreover, Fig. 4 plots the temporal evolution of the yaw rate throughout the trajectory. As mentioned earlier, because of the predictive nature of the MPC controller in a longer term horizon than the IKIBI controller, it is able to anticipate when a big turn is required and starts steering the vehicle earlier than the other controller analyzed.
As a consequence, the trajectory whose input references were generated by MPC will be smoother. Moreover, MPC can explicitly control the feeling of comfort experienced by the vehicle passengers using expression (14). Since this expression acts by limiting the yaw rate, the driving experience will be more satisfying when using MPC rather than the IKIBI controller.
Finally, Table 1 shows the performance cost indexes for each of the controllers in this fast, noiseless test. It can be seen how, by explicitly considering the physical limitations of the vehicle such as the maximum front-wheel steering angle over a prediction horizon, the MPC is able to follow the reference path more accurately than its IKIBI counterpart.

Fast sensor feedback test with noise using EKF
Process and measurement noises are present in a real scenario for this lane-keeping application. Unfortunately, the previous test was observed to turn unstable if these noises are present. Thus, it is justified the use of EKF. Figure 5 plots the planar coordinates of the trajectories in the case where both these noises are present and an EKF is implemented. As mentioned, since using an EKF is essential to have a stable trajectory, we will not show the unstable results for the tests that did not consider using the EKF.
The behavior seen in Fig. 5 is analogous to the former experiment that did not consider noises: the behavior degrades when increasing the longitudinal velocity of the vehicle and the IKIBI controller is saturated. On the other hand, MPC is still able to control the system from this velocity. Table 2 shows a quantitative version of what has been graphically presented in Fig. 5. The MPC controller allows a more accurate lane-keeping behavior when compared to the proposed IKIBI controller, and this is accentuated the more extreme the situation is: in the presence of measurement and process noises and with high longitudinal velocities.

Noiseless, slow sensor feedback test
Nonetheless, the most relevant situation occurs when sensor measurements are not updated every T, but they are updated every MT (here, M = 10). In this situation, the controllers have to be calculated M times slower than in the previous situations. This test explores the situation where no EKF is used and there is no measurement or process noise.
For the IKIBI controller, this situation will necessarily involve keeping the control action constant throughout MT. However, MPC is capable of acting differently. Even though usually MPC calculates a control sequence over a whole prediction horizon but only the first control action of these sequence is applied, it is also possible to apply the different control actions of the control horizon if the update rate of the MPC calculations is not fast enough. Figure 6 shows the comparison between an IKIBI controller calculated every MT and an MPC controller that is calculated every MT but updates its control signal every T because it uses its entire control horizon.
The disadvantage of this implementation strategy for the MPC controller is that the anticipation ability of MPC is lost, especially in this application where the control horizon is equal to the prediction horizon. As a consequence, the lane-keeping behavior degrades as seen in Table 3.
However, the MPC strategy is still a more accurate option that the IKIBI controller because of its ability to explicitly constraint physical variables such as the steering angle of the front wheels.

Slow sensor feedback test with noise using DREKF
Finally, we also considered the situation where the sensor feedback was obtained at a slow rate (every MT) and there was process and measurement noises. The initial test performed in these conditions was to analyze the behavior of each of the two controllers when a Single-Rate EKF (SREKF) was used with a slow sampling frequency. The controllers where also meant to be calculated every MT. However, neither of the two controller strategies (MPC and IKIBI) was able to produce a stable lane-keeping behavior in this situation.
Thus, it is necessary the use of a Dual-Rate EKF (DREKF). DREKF has the ability to provide new measurements every T while only updating its internal matrices and acquires measured variables every MT. Figure 7 shows the results for implementing the DREKF to calculate both controllers every T while only receiving new sensor data every MT. Moreover, Table 4 shows the cost indexes for this experiment. It can be seen how the DREKF allows an accurate lane-keeping behavior in situations where only slow and noisy sensor feedback is available.

Conclusions
The formulation of the Model Predictive Control problem is especially well-suited for controlling self-driving cars since it is able to take into consideration long prediction horizons that will be especially important in the event of abrupt maneuvers and in the presence of measurement and process noise. Additionally, the physical limitations of the vehicle can be explicitly considered and the comfort of the passengers can be directly taken into consideration by using this control scheme.
For these reasons, MPC provides a more accurate lane-keeping behavior than an IKIBI control strategy. The difference in the accuracy of each of the two controllers can be quantified by the cost indexes introduced in Section 4.
The use of EKF has been essential to obtain a stable behavior of the system in this application when measurement and process noises are present. If the update rate of the sensor data is fast enough, it will suffice to use a standard EKF, called SREKF in this work.
However, if the update rate of the sensor feedback is too slow, a DREKF should be used, since it will allow to obtain new sensor data every MT while providing new variable estimations every T to the controllers so that they can be calculated at a fast rate.
One alternative to the use of a DREKF would be to use all the input sequence of the control horizon when calculating the MPC controller every MT. Nonetheless, it is a suboptimal solution since there is a lost of the anticipation ability which is characteristic of MPC. Also, this alternative is only feasible when noise is not present, which happens scarcely in a real application.
Finally, we observed that including a DREKF allows to obtain a degree of lane-keeping accuracy in a slow and noisy sensor feedback test similar to the one obtained for the test where there was no noise and sensor data was acquired at a fast rate for both proposed controllers.

Appendix A Simulation model
Simulations will be performed using Matlab Simulink's Vehicle Dynamics Blockset. This toolbox is equipped with a Vehicle Body 3 DOF block that implements a rigid twoaxle vehicle body model to calculate longitudinal, lateral, and yaw motion. The dynamic equations of the internal model used in this block are [39]: y = −ẋr + F y f + F yr + F y,ext m (A1) r = aF y f − bF yr + M z,ext I zz (A2) where the external forces that act on the vehicle center of gravity are: F x/y/z,ext = F d,x/y/z + F x/y/z,input (A4) M x/y/z,ext = M d,x/y/z + M x/y/z,input and with F z f = bmg − (ẍ −ẏr)mh + hF x,ext + bF z,ext − M y,ext a + b (A10) F zr = amg + (ẍ −ẏr)mh − hF x,ext + aF z,ext + M y,ext a + b (A11) Moreover, the tire forces can be calculated with the slip angles (α): F xr = F xrt cos(δ r ) − F yrt sin(δ r ) (A16) F yr = −F xrt sin(δ r ) + F yrt cos(δ r ) (A17) The physical variables needed to calculate these equations are: • m, vehicle body mass. • a and b, distance of front and rear wheels, respectively, from the normal projection point of vehicle CG onto the common axle plane. • I zz , vehicle body moment of inertia about the vehicle-fixed z-axis. • C y , cornering stiffness. • µ, wheel friction coefficient. • h, height of vehicle's center of gravity above the axle plane.
Also, subscripts f and r refer to the front and to the rear axles, respectively.