# Efficient Path Tracking Control for Autonomous Driving of Tracked Emergency Rescue Robot under 6G Network

Authors: Qing Gu; Guoxing Bai; Yu Meng; Guodong Wang; Jiazang Zhang; Lei Zhou
Journal: Wireless Communications and Mobile Computing
 (2021)

---

## Abstract

This paper proposes a path tracking control algorithm of tracked mobile robots based on Preview Linear Model Predictive Control (MPC), which is used to achieve autonomous driving in the unstructured environment under an emergency rescue scenario. It is the future trend to realize the communication and control of rescue equipment with 6G and edge cloud cooperation. In this framework, linear MPC (LMPC) is suitable for the path tracking control of rescue robots due to its advantages of less computing resources and good real-time performance. However, in such a scene, the driving environment is complex and the path curvature changes greatly. Since LMPC can only introduce linearized feedforward information, the tracking accuracy of the path with large curvature changes is low. To overcome this issue, combined with the idea of preview control, preview-linear MPC is designed in this paper. The controller is verified by MATLAB/Simulink simulation and prototype experiment. The results show that the proposed method can improve the tracking accuracy while ensuring real-time performance and has better tracking performance for the path with large curvature variation.


---

## Body

## 1. Introduction

Due to strong mobility and terrain adaptability, tracked mobile robots are widely used in the rescue environment. In the harsh environment of rescue, it is difficult to build a stable and high-quality communication network. Therefore, it is the future trend to use 6G combined with edge cloud to realize the communication and control of rescue equipment. In this framework, a simple and efficient mobile robot control algorithm is conducive to better accomplish the rescue mission. This is because the short control value computing time is conducive to increasing the information sampling frequency between edge and clouds, which can improve the synergistic ability between edge and clouds. In this case, Linear Model Predictive Control (LMPC) is suitable for the path tracking control of rescue mobile robots. Model Predictive Control (MPC) is a multivariable control strategy, which can obtain the optimal control variables in the prediction time domain through the state equation of the controlled object. Since it does not need the precise model and can deal with the system state constraints, compared with other classical control algorithms (such as Fuzzy, linear quadratic regulator (LQR), and sliding mode control (SMC)), MPC has some advantages [1] and has been widely used in the field of vehicle and robot control [2–6]. Among which, Linear Model Predictive Control (LMPC) has the advantage of faster calculation due to the use of model linearization technology. However, different from general road driving, the rescue scene environment is complex, and the curvature of the target path often changes greatly. Since the feedforward information of the LMPC controller is linearized feedforward information, when the curvature of the reference path changes greatly, the feedforward information obtained through linear prediction is not accurate [5]. Therefore, when tracking such a reference path, the accuracy of the LMPC controller is poor. Therefore, how to make the tracked rescue mobile robot follow such a path is an urgent problem.In recent years, the research results of path tracking control based on LMPC have not paid enough attention to this issue [4–17]. However, in the path tracking control based on feedforward-feedback control, the idea of introducing feedforward information through the preview to improve the control accuracy appears. Meng et al. [18] and Xu and Peng [19], respectively, proposed a path tracking control method based on the preview-linear quadratic regulator (LQR). Diao et al. proposed a path tracking control method based on preview-fuzzy control [20]. These studies have proved the effectiveness of introducing feedforward control. However, considering that control methods such as LQR and fuzzy control have no advantages over LMPC in terms of handling system constraints, etc. [21–23], it is still necessary to study the path tracking control method combining preview control and LMPC.In [24], we discussed the possibility of introducing feedforward information into LMPC through preview control. However, in [24], the strategy of reducing the longitudinal speed during steering has also been introduced. Considering that the longitudinal speed is also an important factor affecting the accuracy of path tracking control [5, 25], whether the preview control can improve the accuracy of LMPC-based path tracking control when the longitudinal speed is constant is still a question to be studied.


## 2. LMPC Controller

Firstly, the LMPC controller is designed. The prediction model of the LMPC controller can be designed based on the following kinematics model of tracked mobile robots:(1)ẋ=vcosθẏ=vsinθθ̇=ω,where x, y, and θ are the abscissa, ordinate and heading angle of the plane motion center of the mobile robot, respectively. v and ω are longitudinal velocity and yaw rate, respectively.The specific process of controller design can refer to [25–28]. First, perform Jacobian Linearization on equation (1), that is, perform Taylor expansion on equation (1) at any point x0x0,y0,θ0,v0,ω0 and retain the first-order term. Then, the discretized prediction model can be obtained through discretization:
(2)x~t+1∣t=Ax~t∣t+BΔut∣t,where
(3)x~t∣t=xt∣t−x0yt∣t−y0θt∣t−θ0,Δut∣t=vt∣t−v0ωt∣t−ω0,A=10−Tv0sinθ01v0Tcosθ001,B=Tcosθ0Tsinθ00T,where x~t+i∣t is the ith predictive state at time t, ut+i∣t is the ith control input at time t, and T is control cycle.Assuming that the prediction horizon isNp and the control horizon is Nc, the state variables in the prediction horizon are
(4)x~t+Np∣t=ANpx~t∣t+ANp−1BΔut∣t+⋯+ANp−NcBΔut+Nc−1∣t.The future output of the system can be represented as a matrix(5)Y~t=Ψx~t∣t+ΘΔUt,where
(6)Y~t=x~t+1∣t⋮x~t+Nc∣t⋮x~t+Np∣t,ΔUt=Δut∣t⋮Δut+Nc−1∣t,Ψ=AA2⋯ANc⋯ANpT,Θ=B⋯0AB⋯0⋮⋱⋮ANc−1B⋯BANcB⋯AB⋮⋯⋮ANp−1B⋯ANp−NcB,Equation (5) is the prediction model of the LMPC controller.The objective function in the rolling optimization process can be designed as(7)Jx~t,ΔUt=∑i=1Npx~t+i∣t−x~reft+i∣tQ2+∑i=1NcΔut+i∣tR2,where x~ref is linearized state information for tracking target points and Q and R are the weight matrices of tracking error and control input increment, respectively.Equation (7) can be abstracted as
(8)Jx~t,ΔUt=Y~t−Y~reftQ^2+ΔUtR⌢2,where
(9)Q^=Q⋱QNp×NpR^=R⋱RNc×Nc,because
(10)x~reft∣t=xreft∣t−x0yreft∣t−y0θreft∣t−θ0,where xreft∣t, yreft∣t, and θreft∣t are the abscissa, ordinate, and heading angle of the tracking target point, respectively.As a result, when the reference input increment is zero, there are(11)Y~reft=Ψx~reft∣t.Further
(12)Y~t−Y~reft=Ψx~t∣t−Ψx~reft∣t=Ψx~t∣t−x~reft∣t,Assumption(13)et=x~t∣t−x~reft∣t.Equation (8) can be simplified into a standard quadratic form
(14)Jx~t,ΔUt=12ΔUtTHΔUt+GΔUt+C,where H=ΘTQ^Θ+R^, G=2ΘTQ^Ψet, and C=ΨetTQ^Ψet.Combined with kinematic constraints, the path tracking control problem of mobile robots can be described as a quadratic programming problem(15)minJx~t,ΔUt=12ΔUtTHΔUt+GΔUt+Cs.t.−0.01rad/s≤ω≤0.01rad/s.Equation (15) can be solved using the “quadprog” function embedded in MATLAB, and the solving algorithm adopts the “active-set” method. Then, the sequence of control input increments in the control horizon can be obtained
(16)ΔU∗=Δu∗t∣t,⋯,Δu∗t+Nc∣tT.The control input for the next control cycle is(17)ut∣t=Δu∗t∣t+ut−1∣t.


## 3. Control Mechanism of Preview

Considering that the curvature of the reference path is limited and the mobile robot will not deviate too far from the reference path, there are the nearest points on the reference path to the plane motion center of the mobile robot. As shown in Figure1, the point closest to the plane motion center of the mobile robot on the reference path is the intersection point between the normal line of the reference path passing through the P point and the reference path, which can be set as A0. At the same time, the displacement error and heading error can be defined. P1P2 is the tangent line of the reference path at the point A0, the lateral error ed is the length of the line segment PP0, and the heading error eh is the deviation between the heading of the mobile robot and the heading of the reference path at the point P0.Figure 1
Schematic diagram of the point on the reference path that is closest to the plane motion center of the mobile robot.In the traditional LMPC controller, the pointA0 is the tracking target point in equation (10).As shown in Figure2, if point A1, which is an arc length in front of the point A0 on the reference path, is used as the tracking target point, substituting the abscissa, ordinate, and heading angle of point A1 into equation (10), the preview-LMPC path tracking controller can be obtained. The arc length A0A1⌢ is the preview distance.Figure 2
Schematic diagram of preview point.


## 4. Simulation Verification

The simulation platform is an ordinary desktop computer, the processor is Intel(R) Core(TM) i5-8500 @ 3.00 GHz, the memory is Kingston DDR4 2400 MHz 8 GB, the hard disk is Seagate ST1000DM010-2EP102, and the operating system is Windows 10 Home Chinese Edition. The simulation software uses MATLAB/Simulink R2018b. In this simulation, the main parameters of the controller are shown in Table1.Table 1
The main parameters of the controller.
DescriptionSymbolValuePrediction horizonNp25Control horizonNc25Control cycleT0.05 sThe weight coefficient matrix is set as(18)Q=111R=11.In the simulation, the curvature of the reference path is set to 0.2 m-1. The simulation includes two groups. The former is used to compare the difference between LMPC and preview-LMPC, and the latter is used to test the performance of preview-LMPC when the preview distance is different.


## 5. First Set of Simulations

Figures3–7 show the results. In this simulation, the preview distance of preview-LMPC is 0.75 m. Figure 3 shows the travel trajectory of the mobile robot, and Figure 4 is a partially enlarged view of Figure 3. It can be seen that under the control of preview-LMPC, the trajectory of the mobile robot is the closest to the reference path.Figure 3
Trajectories of the first set of simulations.Figure 4
Partially enlarged view trajectories of the first set of simulations.Figure 5
Yaw rate of the first set of simulations.Figure 6
Displacement error of the first set of simulations.Figure 7
Heading error of the first set of simulations.Figure5 shows the yaw rate. Figures 6 and 7 show the displacement error and heading error. The displacement error is the lateral error between the robot and the reference path, and the heading error is the error between the robot heading and the tangent of the reference path. The maximum absolute value of displacement error of LMPC is 0.3767 m, and the maximum absolute value of heading error is 0.1185 rad. The above variables of preview-LMPC are 0.0333 m and 0.0486 rad, respectively. Compared with LMPC, the maximum absolute value of displacement error is reduced by 91.16%, and the maximum absolute value of heading error is reduced by 58.99%.Figure8 shows the calculation time of each controller in each control cycle. The results show that preview control has no effect on the real-time performance of the controller, and the real-time performance of the preview-LMPC controller can satisfy the control requirements.Figure 8
Computation time in each control period of the first set of simulations.


## 6. The Second Set of Simulations

Figures9–14 show the simulation results of the second group. The simulation tests the performance of the preview-LMPC path tracking controller when the preview distance is 0.5 m, 0.75 m, and 1 m.Figure 9
Trajectories of the second set of simulations.Figure 10
Partially enlarged view trajectories of the second set of simulations.Figure 11
Yaw rate of the second set of simulations.Figure 12
Displacement error of the second set of simulations.Figure 13
Heading error of the second set of simulations.Figure 14
Computation time in each control period of the second set of simulations.Figure9 shows the trajectory of the mobile robot in the simulation. Figure 10 is a partially enlarged view of Figure 9. The simulation results show that the trajectory under preview-LMPC control is the closest to the reference path. Figure 11 shows the yaw rate.Figures12 and 13 show the displacement error and heading error. When the preview distance is 0.75 m, the maximum absolute values of displacement error and heading error are small, while when the preview distance is 0.5 m and 1 m, the maximum absolute values of error are relatively large. Therefore, when the speed and reference path are constant, the preview-LMPC path tracking controller has the optimal preview distance. Figure 14 shows the calculation time of each controller in each control cycle. It can be seen from the figure that the preview distance has no effect on the real-time performance of the controller, and the real-time performance of the preview-LMPC controller can satisfy the control requirements.


## 7. Experimental Verification

The experiment uses the same experimental platform as in [29], as shown in Figure 15. The prototype is a tracked mobile robot produced by Guoxing Intelligent. The main controller uses Advantech ARK-3500 industrial computer, the processor type is Intel Core i5-3610ME, and the frequency is 2.7 GHz. The robot is localized through differential GPS (DGPS). In this experiment verification, the parameters of the controller are consistent with the simulation verification, as shown in Table 1.Figure 15
Model prototype of tracked mobile robot.Due to the limitation of the experimental site, the speed is set at 1 m/s, and the reference path curvature is set at 0.2 m-1. The preview distance of preview-LMPC is set to 0.75 m. Figures 16–21 show the simulation results.Figure 16
Trajectories of the experiment.Figure 17
Yaw rate of the experiment.Figure 18
Displacement error of the experiment.Figure 19
Heading error of the experiment.Figure 20
Lateral speed of the experiment.Figure 21
Computation time in each control period of the experiment.Figure16 shows the trajectory of the tracked mobile robot. The results show that the robot under the control of preview-LMPC is the closest to the reference path. Figure 17 shows the yaw rate.Figures18 and 19 show the displacement error and heading error. The maximum absolute value of displacement error of LMPC is 0.3471 m, and the maximum absolute value of heading error is 0.1317 rad. The above state variables of preview-LMPC are 0.0477 m and 0.0636 rad, respectively. Therefore, in the experimental results, compared with LMPC, the preview-LMPC reduces the maximum absolute value of displacement error by 86.26% and reduces the maximum absolute value of heading error by 51.71%.Figure20 shows the lateral speed. Due to the low speed and centrifugal acceleration, the lateral speed of tracked mobile robot controlled by each controller is small, that is, no severe sideslip occurs. Figure 21 shows the calculation time of the controller in each control cycle. It can be seen from the figure that the preview distance has no effect on the real-time performance of the controller, and the real-time performance of the preview-LMPC controller can satisfy the control requirements.


## 8. Conclusions

(1)
According to the simulation and experimental results, the introduction of preview control can significantly improve the accuracy of the LMPC path tracking controller. In the simulation, the preview-LMPC can reduce the maximum absolute value of displacement error and heading error by 91.16% and 58.99% compared with LMPC. In the experiment, it can be reduced by 86.26% and 51.71%, respectively(2)
According to the second group of simulations, when the speed and the reference path are constant, the preview-LMPC based path tracking controller has the optimal preview distance. Simulation and experiment show that the accuracy of path tracking control is the best when the preview distance is 0.75 m. In addition, the different preview distance has no effect on the real-time performance of the path tracking controller, and the real-time performance of the preview-LMPC controller can satisfy the control requirements



---

*Source: 5593033-2021-04-16.xml*