A New Path Tracking Method Based on Multilayer Model Predictive Control

Featured Application: This work is used for the unmanned driving of vehicles and other devices. Abstract: At present, many path tracking controllers are unable to actively adjust the longitudinal velocity according to path information, such as the radius of the curve, to further improve tracking accuracy. For this problem, we propose a new path tracking framework based on model predictive control (MPC). This is a multilayer control system that includes three path tracking controllers with ﬁxed velocities and a velocity decision controller. This new control method is named multilayer MPC. This new control method is compared to other control methods through simulation. In this paper, the maximum values of the displacement error and the heading error of multilayer MPC are 92.92% and 77.02%, respectively, smaller than those of nonlinear MPC. The real-time performance of multilayer MPC is very good, and parallel computation can further improve the real-time performance of this control method. In simulation results, the calculation time of multilayer MPC in each control period does not exceed 0.0130 s, which is much smaller than the control period. In addition, when the error of positioning systems is at the centimeter level, the performance of multilayer MPC is still good.


Introduction
An autonomous vehicle system usually consists of mapping, positioning, perception, and navigation. Path tracking is a core system of the autonomous navigation system. Its task is to control the vehicles devices to track the reference path by minimizing errors [1][2][3][4]. Based on our previous work [5], this paper still focuses on the path tracking of articulated vehicles. Of course, the control method proposed in this paper can also be applied in the path tracking of mobile devices such as front-wheel steering vehicles and differential robots.

Motivations
The motivation for this paper is to further improve the accuracy of path tracking. In previous work, we found that most path tracking methods cannot adjust the velocity of the vehicle according to path information such as the radius of the curve, and a high reference velocity at corners will lead to a sharp deterioration in path tracking performance [5].
There is no doubt that path tracking controllers perform well at a low reference velocity. However, too low driving efficiency is also unacceptable in the mining industry. So, when should the articulated vehicle slow down? How much should it reduce its velocity by? It is difficult to calculate the most suitable reference velocity for each curve. For the above problems, we propose multilayer

Contributions
In this paper, our contributions are listed as follows: Firstly, the multilayer MPC controller that can automatically adjust the velocity according to path information is proposed. Our proposed controller is compared with the switching MPC controller and the nonlinear MPC controller. The performance of the multilayer MPC controller is very good. Secondly, two controller topology schemes are proposed, and we prove that the parallel layout can further improve the real-time performance of multilayer MPC. Thirdly, the performance of the multilayer MPC controller under the impact of positioning error is verified.
The content of this paper is arranged as follows: The multilayer MPC-based controller of articulated vehicles is introduced in Section 2. In Section 3, the simulation results are detailed. Finally, in Section 4 we conclude the present work.

Controller Design
The design of the controller consisted of three steps. First, we propose a framework for multilayer MPC, then, we design the first layer controllers and finally the second layer controller.

The Framework for Multilayer MPC
In our previous work [5] we found that most path tracking controllers can only adapt to different reference velocities and cannot regulate the operation velocity automatically. The reason for this phenomenon is that in many path tracking controllers, the state of the path tracking systems is partly determined by the reference velocity. The performance of the controller varies greatly at different reference velocities. We guess that it is possible to get control inputs at different reference velocities through several controllers. These controllers are the first layer of the multilayer MPC, and we name them as path tracking controllers with a fixed velocity. As shown in Figure 1, we set up three path tracking controllers with fixed velocities in the multilayer MPC framework.
Appl. Sci. 2019, x, x FOR PEER REVIEW 3 of 18 controllers are the first layer of the multilayer MPC, and we name them as path tracking controllers with a fixed velocity. As shown in Figure 1, we set up three path tracking controllers with fixed velocities in the multilayer MPC framework. In these three controllers: is the reference velocity in the last control period, and δ is the acceleration amplitude or deceleration amplitude based on constraints and control period. In addition: where lim v  is the acceleration limit and T is the control period.
These three controllers can generate three sets of control inputs. Due to constraints such as the upper limit of the articulated angle speed, sometimes these inputs cannot completely eliminate errors between the articulated vehicle and the reference path. By comparing these residual errors it is possible to decide which set of control inputs can make the path tracking control system perform better. Therefore, the second layer controller is a velocity decision controller.

Path Tracking Controller with a Fixed Velocity
Linear model predictive control (linear MPC) is very powerful in terms of its real-time performance, and it also has high accuracy when the reference velocity is low [26]. Consequently, we chose the linear MPC controller as the path tracking controller with a fixed velocity. In the process of designing this controller, we also referred to the work of Gong et al. [27] and Plessen et al. [28]. The linear MPC controller includes two parts, the predictive model and the rolling optimization. Except for the reference velocities, the three linear MPC controllers are identical.
The basis of the predictive model is the kinematic model of articulated vehicles. We still use the model in [5]. This model is very mature and its application is very extensive. In this kinematics model, In these three controllers: where v re f (t|t) is the reference velocity in the current control period, v re f (t − 1|t) is the reference velocity in the last control period, and δ is the acceleration amplitude or deceleration amplitude based on constraints and control period. In addition: where . v lim is the acceleration limit and T is the control period. These three controllers can generate three sets of control inputs. Due to constraints such as the upper limit of the articulated angle speed, sometimes these inputs cannot completely eliminate errors between the articulated vehicle and the reference path. By comparing these residual errors it is possible to decide which set of control inputs can make the path tracking control system perform better. Therefore, the second layer controller is a velocity decision controller.

Path Tracking Controller with a Fixed Velocity
Linear model predictive control (linear MPC) is very powerful in terms of its real-time performance, and it also has high accuracy when the reference velocity is low [26]. Consequently, we chose the linear MPC controller as the path tracking controller with a fixed velocity. In the process of designing Appl. Sci. 2019, 9, 2649 4 of 18 this controller, we also referred to the work of Gong et al. [27] and Plessen et al. [28]. The linear MPC controller includes two parts, the predictive model and the rolling optimization. Except for the reference velocities, the three linear MPC controllers are identical.
The basis of the predictive model is the kinematic model of articulated vehicles. We still use the model in [5]. This model is very mature and its application is very extensive. In this kinematics model, we ignore the force of the tires and consider the bodies of the articulated vehicle as rigid bodies. The kinematics model is presented in Figure 2. The definition of the parameters is presented in [5]. we ignore the force of the tires and consider the bodies of the articulated vehicle as rigid bodies. The kinematics model is presented in Figure 2. The definition of the parameters is presented in [5]. The matrix form of the kinematic model is: where γ ω is the articulated angular speed.
Carrying out Jacobian linearization on Equation (3), the result is: where: In the path tracking controller with a fixed velocity, the reference velocity is the longitudinal velocity, so: The matrix form of the kinematic model is: where ω γ is the articulated angular speed. Carrying out Jacobian linearization on Equation (3), the result is: where: In the path tracking controller with a fixed velocity, the reference velocity is the longitudinal velocity, so: Appl. Sci. 2019, 9, 2649 5 of 18 In Equations (5) and (6), x(k + i|k) is the i-th predicted state at the current time k, and u(k + i|k) is the i-th possible control input at the current time k. Especially, when i= − 1, x and u are the state and control input of the last control period, respectively.
If the prediction horizon is N p , the control horizon is N c : To make the relationship clearer, the future output of the system can be represented in a matrix: where: Since the function of the controllers is to enable articulated vehicle to track the reference path as quickly and smoothly as possible, the objective function of the rolling optimization can be designed as: where x re f is the linearized state information of the tracking target point, Q and R are the weight matrices, ε is the relaxation factor, and ρ is the weight factor. We can rewrite Equation (15) as: The tracking target point is the closest point to the articulated vehicle which is selected on the reference path. We can set its status information to x f re f (k|k), y f re f (k|k), θ f re f (k|k), and γ re f (k|k). Then, we can set that: So: Then Equation (16) can be rewritten as: If: We can rewrite Equation (19) as: where: Combined with the constraints in [5], the final rolling optimization function is: We can use the quadprog function in MATLAB to solve Equation (23), and the solution algorithm can be specified as active-set. After the solution, the control input increment sequence in the control time domain can be obtained:

Velocity Decision Controller
The function of the velocity decision controller is to judge the three sets of control inputs. This controller is not required to stand out in terms of real-time performance, as there is no large-scale optimization solution. Instead, this controller is required to accurately predict the state of the articulated vehicle in a long time domain. According to these requirements, we believe that nonlinear MPC is well suited for velocity decision controllers.
The kinematics model detailed in Equation (3) can be abbreviated as: where: After discretization, a nonlinear prediction model can be obtained: Substituting the three sets of control inputs obtained in the first layer into Equation (27), the corresponding future state of the articulated vehicle can be obtained. The errors between the reference path and the future state of articulated vehicles are: where N vp is the prediction horizon in the velocity decision controller and x re f is the information on the reference path. x re f is obtained by the interpolation of the reference path, based on the reference velocity and control period. The arc length on the reference path between x re f (k + i|k) and x re f (k + i + 1|k) is equal to the product of the reference velocity and the control period. Based on the above prediction model, the following decision function can be designed: In order to ensure safety, the path tracking controller with the slowest velocity is set at the highest priority. The judgment rules are as follows: where µ 1 and µ 2 are relaxation factors.

Simulation
The simulation environment used in this paper is exactly the same as in our previous work [5]. The parameters of the model and the controller are shown in Table 1. Figure 3 shows the definition of displacement error and heading error. Their detailed explanation can be found in [5]. The simulation was divided into three groups. The first group is the comparison between the multilayer MPC controller, the nonlinear MPC controller and the switching MPC controller. The second group is the comparison between the two topologies of the multilayer MPC controller, and the third group is a performance test of the controller with positioning errors. In all simulations, most of the parameters were fixed. Each group simulation had the same computer model of articulated vehicles and the same reference path. The reference path consisted of straight lines and arcs, where the radius of the arcs was 10 m. The radius of these arcs is 5 m smaller than the radius of the arcs in [5].

Comparison Between Controllers
Firstly, multilayer MPC, switching MPC, and nonlinear MPC are compared. The reference velocity of the multilayer MPC controller was set to 5 m/s, and that of the other two controllers was set to 2.5 m/s. Figure 4 shows the tracking results. Multilayer MPC and switching MPC successfully completed the control task, however, switching MPC failed. According to [5,[16][17][18], switching MPC can complete path tracking when the reference velocity is low or the radius of the reference path is large.

Comparison Between Controllers
Firstly, multilayer MPC, switching MPC, and nonlinear MPC are compared. The reference velocity of the multilayer MPC controller was set to 5 m/s, and that of the other two controllers was set to 2.5 m/s. Figure 4 shows the tracking results. Multilayer MPC and switching MPC successfully completed the control task, however, switching MPC failed. According to [5,[16][17][18], switching MPC can complete path tracking when the reference velocity is low or the radius of the reference path is large.

Comparison Between Controllers
Firstly, multilayer MPC, switching MPC, and nonlinear MPC are compared. The reference velocity of the multilayer MPC controller was set to 5 m/s, and that of the other two controllers was set to 2.5 m/s. Figure 4 shows the tracking results. Multilayer MPC and switching MPC successfully completed the control task, however, switching MPC failed. According to [5,[16][17][18], switching MPC can complete path tracking when the reference velocity is low or the radius of the reference path is large.    Figures 5-7 show the longitudinal velocity, articulated angle, and articulated angle speed, respectively. All control inputs and states are within the system constraints. The displacement error is shown in Figure 8. The heading error is shown in Figure 9. The errors of switching MPC are divergent. The maximum errors of the other controllers are shown in Table 2. The maximum displacement and heading errors of multilayer MPC are 92.92% and 77.02% smaller than those of nonlinear MPC, respectively.
Multilayer MPC shows strong real-time performance, as shown in Figure 10. Its maximum calculation time in each control period was 0.0130 s, and its average calculation time was 0.0086 s. The real-time performance of multilayer MPC is significantly better than that of nonlinear MPC. Of course, the best control method for real-time performance is switching MPC, but this control method cannot control the articulated vehicle tracking reference path under this simulation condition.   Multilayer MPC shows strong real-time performance, as shown in Figure 10. Its maximum calculation time in each control period was 0.0130 s, and its average calculation time was 0.0086 s. The real-time performance of multilayer MPC is significantly better than that of nonlinear MPC. Of course, the best control method for real-time performance is switching MPC, but this control method cannot control the articulated vehicle tracking reference path under this simulation condition.    By comparing these control methods, we can see that multilayer MPC has good control precision and real-time performance. From a theoretical point of view, this controller can automatically adjust velocity according to the reference path.   By comparing these control methods, we can see that multilayer MPC has good control precision and real-time performance. From a theoretical point of view, this controller can automatically adjust velocity according to the reference path.   By comparing these control methods, we can see that multilayer MPC has good control precision and real-time performance. From a theoretical point of view, this controller can automatically adjust velocity according to the reference path.   Multilayer MPC shows strong real-time performance, as shown in Figure 10. Its maximum calculation time in each control period was 0.0130 s, and its average calculation time was 0.0086 s. The real-time performance of multilayer MPC is significantly better than that of nonlinear MPC. Of course, the best control method for real-time performance is switching MPC, but this control method cannot control the articulated vehicle tracking reference path under this simulation condition.

Sequential Computation and Parallel Computation
Multilayer MPC is a distributed control method, so there are multiple topologies. The simplest two are sequential and parallel. These two calculation processes are shown in Figure 11. Of course, multi-threaded computing hardware is the basis of parallel computation. By comparing these control methods, we can see that multilayer MPC has good control precision and real-time performance. From a theoretical point of view, this controller can automatically adjust velocity according to the reference path.

Sequential Computation and Parallel Computation
Multilayer MPC is a distributed control method, so there are multiple topologies. The simplest two are sequential and parallel. These two calculation processes are shown in Figure 11. Of course, multi-threaded computing hardware is the basis of parallel computation.

Sequential Computation and Parallel Computation
Multilayer MPC is a distributed control method, so there are multiple topologies. The simplest two are sequential and parallel. These two calculation processes are shown in Figure 11. Of course, multi-threaded computing hardware is the basis of parallel computation. From a theoretical point of view, sequential computation and parallel computation only have an effect on real-time performance but have no effect on control accuracy. In a trajectory figure similar to Figure 4, it is difficult to observe the difference between the two topologies.. So, in this group of simulations, we will compare the control inputs and states. Figures 12, 13, 14, 15, and 16 show the longitudinal velocity, articulated angle, articulated angle speed, displacement error, and heading error, respectively. In theory, the variables of the two topologies should be identical. In the simulation, there were some deviations on a small scale. This phenomenon is caused by factors such as the sampling interval in the simulation. From a theoretical point of view, sequential computation and parallel computation only have an effect on real-time performance but have no effect on control accuracy. In a trajectory figure similar to Figure 4, it is difficult to observe the difference between the two topologies. So, in this group of simulations, we will compare the control inputs and states. Figures 12-16 show the longitudinal velocity, articulated angle, articulated angle speed, displacement error, and heading error, respectively. In theory, the variables of the two topologies should be identical. In the simulation, there were some deviations on a small scale. This phenomenon is caused by factors such as the sampling interval in the simulation.             The real-time performance of parallel computation is obviously better than that of sequential computation, as shown in Figure 17. The maximum computation time for parallel computation was 0.0090 s, which was 30.77% smaller than that of sequential computation. The average computation time for parallel computation was 0.0044 s, which was 48.84% less than that of the sequential computation.  The real-time performance of parallel computation is obviously better than that of sequential computation, as shown in Figure 17. The maximum computation time for parallel computation was 0.0090 s, which was 30.77% smaller than that of sequential computation. The average computation time for parallel computation was 0.0044 s, which was 48.84% less than that of the sequential computation.  The real-time performance of parallel computation is obviously better than that of sequential computation, as shown in Figure 17. The maximum computation time for parallel computation was 0.0090 s, which was 30.77% smaller than that of sequential computation. The average computation time for parallel computation was 0.0044 s, which was 48.84% less than that of the sequential computation.

Performance with Positioning Error
The purpose of this group is to verify the performance of the controller with positioning errors. At present, positioning devices such as BDM670 of Beijing BDStar Navigation Co. Ltd can provide centimeter-level positioning. Consequently, we added a random error within ± 0.01 m to the horizontal and vertical coordinates of the articulated vehicle model. Since the judgment of Equation (30) is based on errors, positioning errors will affect the function. To avoid the articulated vehicle always traveling at the minimum of the reference velocity, we increased µ 1 and µ 2 . In this group, µ 1 was 4 and µ 2 was 2. Figures 18-24 show the results of this group of simulations. The trend of each variable with positioning error was the same as the trend of the ideal variable. Therefore, we believe that multilayer MPC can be used in a working environment with small positioning errors.

Performance with Positioning Error
The purpose of this group is to verify the performance of the controller with positioning errors.

Performance with Positioning Error
The purpose of this group is to verify the performance of the controller with positioning errors.             In addition, multilayer MPC still faces some challenges. As Figure 21, The articulation angle speed is not smooth due to the influence of the positioning error. Of course, this is a phenomenon that cannot be avoided by any controller, but we think there is still the possibility of improvement here. We believe that nonlinear MPC is more robust than linear MPC. If the linear MPC-based path tracking controller in multilayer MPC is replaced with the nonlinear MPC controller, the amplitude of the articulated angle speed may be reduced. However, the real-time performance of nonlinear MPC is suboptimal, requiring further work in the future solve this problem.   Figures 18,19,20,21,22,23,and 24 show the results of this group of simulations. The trend of each variable with positioning error was the same as the trend of the ideal variable. Therefore, we believe that multilayer MPC can be used in a working environment with small positioning errors.
In addition, multilayer MPC still faces some challenges. As Figure 21, The articulation angle speed is not smooth due to the influence of the positioning error. Of course, this is a phenomenon that cannot be avoided by any controller, but we think there is still the possibility of improvement here. We believe that nonlinear MPC is more robust than linear MPC. If the linear MPC-based path tracking controller in multilayer MPC is replaced with the nonlinear MPC controller, the amplitude of the articulated angle speed may be reduced. However, the real-time performance of nonlinear MPC is suboptimal, requiring further work in the future solve this problem.

Conclusions
In our previous work [5] we found that switching model predictive control (MPC), nonlinear MPC, and most other control methods can operate at different longitudinal velocities but cannot actively adjust the longitudinal velocity according to the given path information. However, when tracking the same reference path, the tracking accuracy can be significantly improved by reducing the longitudinal velocity. To solve this problem, we propose a new MPC control method and name it multilayer MPC.
From the perspective of science, our contribution is to propose an MPC control framework that can actively adjust the longitudinal velocity according to the path information when path tracking. This is an exploration and development of MPC. We think that this framework can be applied to the control of other automated equipment.

Conclusions
In our previous work [5] we found that switching model predictive control (MPC), nonlinear MPC, and most other control methods can operate at different longitudinal velocities but cannot actively adjust the longitudinal velocity according to the given path information. However, when tracking the same reference path, the tracking accuracy can be significantly improved by reducing the longitudinal velocity. To solve this problem, we propose a new MPC control method and name it multilayer MPC.
From the perspective of science, our contribution is to propose an MPC control framework that can actively adjust the longitudinal velocity according to the path information when path tracking. This is an exploration and development of MPC. We think that this framework can be applied to the control of other automated equipment.
From an engineering perspective we present many contributions. Firstly, we have demonstrated that the accuracy of path tracking can be improved by adjusting the longitudinal velocity. Compared with the nonlinear MPC controller, the multilayer MPC controller, which can actively adjust the longitudinal velocity, reduces the maximum displacement error by 92.92% and reduces the maximum heading error by 77.02%. Secondly, the real-time performance of multilayer MPC is very good. Moreover, compared with sequential computation, multilayer MPC with parallel computation has better real-time performance and its maximum calculation time is 0.0090 s. Thirdly, the multilayer MPC controller can still perform the path tracking task under the impact of positioning error.
Of course, there are still some shortcomings in our work. Firstly, limited by the lack of equipment, multilayer MPC has not been verified by real vehicles. We are deeply sorry about this issue, but the problem of acquiring or developing experimental equipment is difficult to solve quickly. We will work hard and try our best to provide the experiment as soon as possible. In addition, for positioning errors, the robustness of linear MPC in multilayer MPC does not satisfy us. We will solve this problem in our future work.

Conflicts of Interest:
The authors declare no conflict of interest. The funders had no role in the design of the study, in the collection, analyses, or interpretation of data, in the writing of the manuscript, or in the decision to publish the results.