Strojniški vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 © 2019 Journal of Mechanical Engineering. All rights reserved. D0l:10.5545/sv-jme.2019.5980 Original Scientific Paper Received for review: 2019-01-04 Received revised form: 2019-04-14 Accepted for publication: 2019-05-07 Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control Lin Zhou - Guoqiang Wang* - Kangkang Sun - Xin Li Jilin University, School of Mechanical and Aerospace Engineering, China This paper proposes a model predictive control (MPC) algorithm for trajectory tracking of vehicles. Using MPC can reduce tracking errors and random disturbances in complex environments in time. According to the linear kinematics model of the vehicle, a kinematics trajectory tracking controller and an electromechanical coupling dynamics trajectory tracking controller are designed. The drive system of the electrically driven tracked vehicle is non-linear, and an electromagnetic system and mechanical system interact with each other. Taking the electromechanical coupling characteristics into consideration can ensure the matching of the electromechanical performance and the stability of the system during the trajectory tracking control. To verify the algorithm, kinematic simulations and dynamic simulations are performed. The simulation results show that the algorithm has good tracking ability. In addition, a set of test devices is designed to confirm the performance of the trajectory-tracking control algorithm in a real environment. Vision recognition is used to obtain vehicle deviation, and the Kalman filter is used to reduce signal interference. The result shows that the algorithm can meet trajectory tracking requirements. Keywords: model predictive control, trajectory tracking, tracked vehicle, electromechanical coupling Highlights • A kinematics model, a dynamic model, and an electromechanical coupling model of the tracked vehicle are established. • The control system in trajectory tracking is designed based on model predictive control. • The electromechanical coupling characteristic is added to the trajectory tracking control. • The simulation and vehicle experiment results further validate the control algorithm. 0 INTRODUCTION A track vehicle has good passing ability, low grounding pressure, and high adhesion to the ground. It can work in harsh environments, such as soft land and wetlands. Especially in dangerous or narrow situations, a crawler travel mechanism is often used, such as mine-sweeping robots, fire-fighting robots, and deep-sea mining robots [1] and [2]. Automatic navigation or trajectory-tracking technology for track vehicles is necessary for these vehicles. [3] and [4] The automatic movement of the vehicle mainly refers to automatic control the vehicle to reach the designated target, according to the predetermined reference trajectory and the state of the vehicle and the environmental information [5]. Furthermore, trajectory tracking requires the controlled object to reach the specified reference point within a given time [6]. Vehicle mathematical model analysis plays a key role in solving the trajectory tracking control problem of unmanned track vehicles. However, track vehicles are complex nonlinear systems, and it is difficult to establish accurate mathematical models. At the same time, high-precision control problems need to consider many uncertain factors, which can lead to a high complexity of control problems. Relevant scholars have used different control strategies to explore the trajectory tracking of unmanned track vehicles. Yeu et al. use a modified proportional-differential controller for the path tracking of soft road track vehicles [7]. Zou et al. use a backstepping method based on a modified proportional-integral-derivative (PID) computed-torque control for unmanned track vehicle tracking on hard ground [8]. Huang et al. use a PI path-tracking controller based on look-ahead point information for a differential-drive crawler-type robot [9]. Chen et al. [10] and Li et al. [11] use adaptive sliding mode control theory to control wheeled vehicles. Other scholars also have used other methods, such as fuzzy logic-based controllers [12], adaptive neural-fuzzy inference controllers [13] and slip-compensating control strategies [14]. In the methods commonly used for trajectory tracking control, PID control has poor control effect on nonlinear systems and structurally uncertain systems due to fixed control parameters. Optimal control and pure tracking control depend on specific mathematical models. Fuzzy control does not need to establish an accurate mathematical model, so it is suitable for nonlinear time-varying systems, but the choice of fuzzy rules is not systematic, and it is difficult to adjust online. Model predictive control (MPC) is based on the system using rolling optimization techniques to solve constrained real-time problems. Predictive control has clear basic ideas and strong development *Corr. Author's Address: Jilin University, School of Mechanical and Aerospace Engineering, China, linzhou17@mails.jlu.edu.cn 329 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 capabilities. It can be combined with various system models, control theory and optimization algorithms to form various control algorithms. MPC can overcome tracking errors and random disturbances in complex environments, and eliminate the uncertainty caused by model mismatch and external disturbances in time; it is also suitable for systems in which mathematical models are inaccurate and have constraints. Consequently, MPC is widely used in the motion control of smart cars, drones and mobile robots [15] and [16]. Shen et al. performed trajectory tracking control of autonomous underwater vehicle using Lyapunov-based model predictive control [17]. Falcone et al. applied MPC to an active front steering system in an autonomous vehicle [18]. Based on the vehicle dynamics model, Lee et al. [19] and Kim et al. [20] applied the MPC method to laterally control a vehicle and achieved good control results. Researchers studied the trajectory control method of a tractor-trailer system based on MPC, and the tests proved that the control method is effective [21] and [22]. Therefore, applying the model predictive control method to the electric drive track vehicles can achieve good control effects, but at this stage there is less research in this direction. The electromechanical coupling system of the electrically driven track vehicle consists of an alternating current motor and a working machine. The electromagnetic system and the mechanical system interact with each other and form a complex nonlinear system. When the electromechanical performance is not suitable, the induction motor is likely to be blocked and cannot drive the crawler device. In severe cases, it may even burn out the motor [23]. Therefore, it is necessary to perform electromechanical coupling dynamics analysis when analysing and controlling a track vehicle. In the current research, many scholars have established kinematic and dynamic models of track vehicles and applied them to motion analysis [24] and [25]. However, there are few studies combining vehicle trajectory tracking control with electromechanical coupling dynamic analysis. This paper establishes the kinematic model, the dynamic model, and the electromechanical coupling dynamic model of track vehicles. According to the kinematics model, it designs a trajectory tracking algorithm based on the model predictive control. In the simulation environment, the kinematics model and the electromechanical coupling dynamics model are tested under two conditions to verify the correctness of the control algorithm. To verify the effectiveness of the algorithm under real conditions, physical prototype tests of the track vehicle based on monocular vision and Kalman filter under different initial conditions are performed. 1 ELECTROMECHANICAL COUPLING MODEL OF A TRACK VEHICLE 1.1 Kinematics Model When the track is steering in a plane without any slippage, as shown in Fig. 1, according to the triangular relationship: (1) * ± * * T * 2 2 Fig. 1. Track steering schematic diagram According to Eq. (1), the steering radius, the centre-of-mass velocity, and the steering angular velocity of the track can be obtained: R = B. VR + VL 2 h -vl\ ^ = ic = vr-vl c R B (2) (3) (4) To obtain the plane motion equation of the track vehicle, the ground coordinate system OXY is established, as shown in Fig. 2. When vr or v l is consistent with the vehicle's heading direction, it takes a positive sign; otherwise, it is negative. According to Fig. 2, the differential equation of motion of the track vehicle is as follows: vR + v, x = vc ■ cosp = —-— ■ cosp y = vc ■ smp = - ■ cosp. (5) p = mc =- B VR _ VL Vc = 2 2 330 Zhou, L. - Wang, G. - Sun, K. - Li, X. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 Fig. 2. Plane motion analysis Take x = (x, y, z)T as the state vector and u = (vR, vL)T as the input vector. Eq. (5) can be rewritten as a state equation in matrix form: 1 1 -cos^ —cos^ x 1 1 vR y = -smV ^sin^ ' • (6)

= Tem - Tl - f>. (10) Joint Eqs. (7) to (10): T- T r (8mV , Jc®c , 17 , 77 | ^ J®R = TemR —\ —T^ + + Frr sr f®R n v 2 B t ■ t r (dmv J® . J®L = Tm--\ —---— + Fr, +Fs, \- f® . (11) in v 2 b Bring Eqs. (3) and (4) into Eq. (11) and calculate: ®R = -tfm®R - UL®L + tTemR - t Frr n (12) - t^Fr + uTemL - U ^ Frl - U^FS1 n n n r ®L = -Uf0PR - tf^L + uTemL - U~Frr n r r r - U — Fsr + tTeML - t~FH - t~Fsl n n n The components of t and u in Eq. (12) are expressed as follows: 1 t = — 2 1 1 J + \ f 1 u = — 2 Smr2 1 J+ 2 Jcr1 B2i2 y 1 \ (13) J+ Smr2 J+ 2 Jr1 2i BAiA Take x = (mR,mL,x,y,y)T as the state vector, U = (TemRi / r, Fw, Fsr, TemLi / r, Fr,, Fs, )T as the input. Joint Eqs. (6) and (12) can obtain the complete dynamics equation, and the writing matrix form is as follows: x = Ax+Bu Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 3 (14) 331 + Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 where A = -f -uf -uf -f r — cosö 2i r —cosö 2i r — sinö 2i r —sinö 2i r r Bi - Bi Bt = ± ¡n t —t -tu —u —u u —u —u t —t —t (15) 1.3 Electromechanical Coupling Model To establish the electromechanical coupling model of the track vehicle, a basic dynamic equation of the motor should first be obtained. Take the current, the rotor angle, and the rotor mechanical angular velocity as state variables, and taking the applied voltage of the winding and the load torque on the shaft as the input vector, which are x = (i,mm,0)T and v = (u,7^) . The equation for the induction motor in the ABC coordinate system is: d = - L-1 { R + e I i + L-'u dt dm dt de_ dt de = ^ ^ - Lm - tl. (16) -2 J de J m J = Pomm Since the Eq. (16) is a differential equation with time-varying coefficients, the solution is complicated. If a certain degree of decoupling is achieved with coordinate changes, the equation of state can be simplified. The equation of the motor in the aß - dq system can be expressed as: — = - L-1 (R + G0 )i + L-1u At V ) dt dm dt dd^ dt . = AfGi --II J = Pomrn J J (17) In Eq. (17), both L 1 and G are constant arrays, which is easy to solve. Combining dynamic Eq. (14) of the track vehicle with Eq. (17), the equation for the electromechanical coupling of the track vehicle can be obtained. Tate x = (isaR,isfiR,irdR,irqR,mR,0R,isaL,ispL, irdL,irqL,mL,Ql,x,y,q)T as the state vector and take u = (UsaR , Us!3R , UrdR , UrqR, Frr , Fsr , UsaL, Usl3L, UrdL , UrqL, Frl, Fsl) as the input vector. The induction motor rotor is always short-circuited, so urdR = urqR = urdL = urqL = 0. Then the track vehicle electromechanical coupling equation is as follows: x = Ax + Bv, (18) where A = -L 1 (R + G6r ) 04x1 04x1 04x6 piRG -tf. 0 up0iTLG -uf. 0 01x4 Po 0 01x6 04x6 -L1 (R + G6l ) 04x1 04x1 wR -uf. 0 t^G 01x4 -tf. p0 0 0 015x3 r —cm 2i r —cm 2i 03x4 r —sm 2i r Bi 03x1 03x4 r —sm 2i r - Bi 03x1 (19) B = L-1 r r -t— -t — in in rr -u — -u — in in rr -u — -u — in in L-1 rr -t— -t — in in (20) (21) 2 MODEL PREDICTIVE CONTROL ALGORITHM The discrete-time state-space model of a linear system can be expressed as: fx(k +1) = Ax(k) + Bu(k) |y(k) = Cx(k) • If the state x is measurable and the system is undisturbed, set the prediction time domain to P and the control time domain to M (where P >M). At the sampling instant k, the system control input is u(k). Let the control variable changes only in the future M steps k, k+1, ..., k+M-1, and remain unchanged outside the control time domain, then the future P steps k+1, k+2, ..., k+P-1 are as follow: X(k + j) = A x x(k) + B x U(k), (22) where: 0 332 Zhou, L. - Wang, G. - Sun, K. - Li, X. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 " x(k +1) " " u(k) " u(k +1) X(k + j )= x(k + M ) , U(k) = u(k + 2) x(k + -P) _ u(k + M -1) A, = A Am A P B_ B B . (23) Z AB For the track vehicle kinematics model Eq. (6), it can be written as a general form: X = f (x, u). (24) Using linear systems can make it easy to check local stability and accelerate the simulation. Since the kinematic model is simple in form, and the coefficient matrix elements are constants, the kinematic model should be linearized. Assuming that the target trajectory is the running trajectory of the reference vehicle, and the state quantity and the control amount of the reference vehicle at any time are given, then the target trajectory can be tracked by the deviation between the controlled vehicle and the reference vehicle. Every point on the target trajectory satisfies Eq. (6): X, = f (x,, u,.) (25) In Eq. (25), variable, and = (Xr , yr , Zr )T K = (v„, Vfr ) is the reference state is the reference manipulated variable. Expanding the above equation with a Taylor series at the reference track point and only retaining one degree term: df (x, u) x r = f (x r, ur) + - df (x, u) dx 3x (U - Ur ). (x - xr )- (26) Shortened as: x r = f (x r, ur) + fx,r (x - xr) + fu ,r (u - ur). (27) In Eq. (27), fx,r and fu,r are the Jacobian matrices of f relative to x and u, respectively. Subtracting Eq. (24) and Eq. (27), we obtain: x = A(t )x + B(t )ii. (28) In Eq. (28), x = x - xr and fi = u - ur are deviation vectors; A(t) = fxr, B(i) = fu,r. The above Eq. (28) is a continuous time system, which needs to be discretized to obtain the general predictive control form as Eq. (21). Discretize it with the forward difference: x (k +1) = A(k )x (k) + B(k )ii (k), (29) where, A(k) = I + TA(t) = 1 0 - 2(vR + vl )sin^'t 0 1 - 2(vr + vl )cosv-t 0 0 1 (30) B(k) = TB(t) = T cos^ cos^ 2 2 sin^ sin^ 2 2 1 -1 B -B (31) Predictive control needs to solve an optimization problem in each sampling time, so a performance index function needs to be set in advance. By controlling the amount of optimization, we can obtain the maximum or minimum. The performance index function usually uses a quadratic function, and the normal control objective is to make the output predicted value as close as possible to the target value. The trajectory tracking control involved in this paper belongs to the output optimal control problem, and its performance function can be expressed as: J = XT (k + j)QX(k + j) + UT (k) RU(k). (32) State-weighting matrix Q is a diagonal matrix, Q = diag(ql,q2,...,qp). Each element qi represents the importance of tracking errors at different times; control weighting matrix R is also generally taken as a diagonal matrix. It achieves a certain degree of suppression of the fluctuation of the control variable and improves the stability of the system to some extent. Since the performance index function is a quadratic function, it can be represented as a quadratic programming problem to solve. Write Eq. (32) as the standard form of quadratic programming: J = - Ur (k) H (k )U(k) + f r (k )U(k) + d (k), 2 s.t. U min(k) < U(k) < Umax(k), Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 5 (33) 333 =0 x Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 where: H (k) = 2(Bt (k )QB(k) + Rx), f (k) = 2Bt (k )QA(k )x (k|k), d (k) = x T (k|k) AT (k )QA(k )x (k|k). (34) H is a Hessian matrix, symmetrical and positive definite, and describing the quadratic part of the objective function; vector f describes the linear part of the objective function; d is an unrelated item to U(k) and does not affect the optimization result. Therefore, the optimization control problem can be described as finding a control sequence U*(k), so that: U (k)=argmin J. (35) U (k) After obtaining the optimal sequence, executing the first control amount: u(k) = (1,0,...,0)U*(k). (36) The optimization shown in Eq. (35) is an unconstrained optimization. The fluctuation of prediction output and the control input can be suppressed to some extent by the weighting matrix Q and R. However, the control variable cannot be precisely constrained. In the actual process, if the amplitude of the applied control signal or its increment too large, it can cause a significant impact on the system and even affect the stability of the system. Therefore, for Eq. (33), the boundary constraint of the control variable needs to be considered. u mn(£) = U mm - U , UmaxCk) = Umax - U where Umm and Ur =[ur(k),...,ur(k + N-1)] control sequence. (37) and Umax are control variable constraints is the reference ^Ending ^ Fig. 4. MPC trajectory tracking process Using the solution algorithm to solve Eq. (33) in each control cycle and obtain a control sequence U*(k)={uk (k), uk (k +1),..., uk (k + N -1}. Applying the first control variable uk(k) of the control sequence to the system. Repeat this step to achieve track vehicle trajectory tracking. The process of MPC trajectory tracking control is shown in Fig. 4. 3 NUMERICAL SIMULATION To verify the performance of the control algorithm, the trajectory tracking ability simulation is carried out under the conditions of kinematics and dynamics. 3.1 Kinematic Trajectory Tracking Simulation To perform kinematics tracking simulation, this paper established a kinematics model of the vehicle in MATLAB and built a controller in Simulink. At present, the MPC controller is based on the present position and uses the model to simulate several kinds of outputs, and the optimizer finds the optimal one. After obtaining the optimal control sequence, the controller applies the first step to the vehicle. Then, it calculates the position at the next moment using Eq. (6) and uses it as the starting point for the new optimization. It repeats at this process until the control is completed. 3.1.1 Straight Linear Conditions When the track vehicle is in line tracking, the reference trajectory equation is set to: ' x(t) = 0.15t < y(t) = 1 . (38) 9(t) = 0 The reference manipulated variable: vrr (t) = v„ (t) = 0.15. (39) In the simulation process, the sampling time is T = 1 s, the prediction time domain is N = 10, and the simulation time is 50 s. State-weighting matrix q, = diag(1, 1, 0.1)*e!/1°, Q=diag(q1, q, ..., qn), control-weighting matrix R = 0.1 I, the initial condition is x = (0, 0, 0)T. Use the quadprog function in MATLAB to solve the quadratic programming problem and selecting "interior point convex" as the optimization method. The simulation results are shown in Fig. 5. It can be seen from Fig. 5 that the MPC control method enables the vehicle to overcome the influence of the initial deviation when tracking in a straight 334 Zhou, L. - Wang, G. - Sun, K. - Li, X. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 a) b) d) Fig. 5. Straight line tracking result; a) trajectory comparison, b) velocity fluctuation, c) distance error, and d) angle error line, and to achieve accurate tracking in about 25 s, which has a fast convergence speed. After reaching the desired velocity, the vehicle remains stable, the vehicle tracking process is not over-adjusted, and the error remains at zero after tracking. 3.1.2 Spiral Conditions Assuming that the reference vehicle is travelling at a constant velocity on the target spiral trajectory, the spiral trajectory equation is set to: S = Vct 5n 2 j x =| cos-s ds. 144 f 5n 2 , y = cos-s ds Jo 144 -i' c Jo (40) The reference control variable set as: 5n c =-s 144 m = vcc v = v +- (41) mB. 2 mB In Eq. (41), vc=0.12 m/s is the reference vehicle's centre-of-mass velocity. The simulation time is 100 s, and the sampling time and the prediction time domain are the same as the line-tracking settings. The simulation results are shown in Fig. 6. As shown in Fig. 6, the spiral tracking also shows good results. At the beginning of the simulation, there is an angular error of 0.8 rad between the heading of the controlled vehicle and the target heading. With the real-time adjustment of the velocity and heading of the controlled vehicle, the distance error and the angle error are rapidly reduced, and accurate tracking is achieved at 35 s. Although a certain amount of angular overshoot occurs during the tracking process, the overshoot is only about 15 % of the maximum angular error, and the error remains at 0 after accurate tracking. 3.2 Electromechanical Coupling Dynamics Simulation The complete electromechanical coupling dynamics model is the 15-step nonlinear differential Eq. (20), which requires a numerical method to be solves. To ensure the real-time performance of the system, the prediction model of this section still uses the linear kinematics model. At each sampling instant k, obtain the optimal control sequence U*(k) = {u(k), u(k+1), ..., M(k+«-1)>. According to the first item u(k), the left and right motor speed npi, npr can be calculated. Calculate the load torque of the two sides of the electric motor according to c) Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 335 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 a) b) a) b) d) Fig. 6. Spiral line tracking result; a) trajectory comparison, b) velocity fluctuation, c) distance error, and d) angle error d) Fig. 7. Straight-line tracking result; a) trajectory comparison, b) velocity fluctuation, c) distance error, and d) angle error the motor speed, and find the voltage and frequency on both sides, taking the voltage and frequency as the current optimal control amount. According to the electromechanical coupling dynamics Eq. (18), calculate the crawler position of the next sampling moment and use it as the next optimization starting point. 336 Zhou, L. - Wang, G. - Sun, K. - Li, X. c) c) Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 In the following straight-line tracking and spiral tracking, the simulation parameters are the same as the settings in Section 3.1, and the actual trajectory is calculated using the electromechanical coupling dynamics model. 3.2.1 Straight Linear Conditions It can be seen from Fig. 7 that the MPC control method can overcome the influence of the initial y direction deviation under the condition of electromechanical coupling dynamics. The purpose of tracking the target trajectory is achieved by adjusting the voltage and frequency of the motors on both sides. In the trajectory tracking process, the distance error between the controlled vehicle and the target line in the y direction is continuously reduced, and the distance error in the x direction is increased first and then decreased. After 20 s, both errors are reduced to 0, and the vehicle achieves accurate tracking. After that, the vehicle remained stable, and the distance error and the angle error were kept at 0. 3.2.2 Spiral Conditions As shown in Fig. 8, in the spiral trajectory tracking condition, the MPC control method can make the vehicle overcome the influence of the initial distance error and angle error under dynamic conditions. During the tracking process, the voltage and frequency control amount of the right motor has a certain fluctuation, and the left changes smoothly. After 32 s, the tracking is basically realized. The angular error has a certain overshoot during the tracking process, but the distance error and the angle error keep at 0 after the accurate tracking. Based on the above dynamic simulation results and kinematic simulation results, the state-space based model predictive control algorithm can realize the trajectory tracking of the linearized track vehicle model under different path curves and different initial deviation conditions. The trajectory tracking result is good, which can meet the requirements of trajectory tracking control of unmanned track vehicles. Adding the motor characteristics into the trajectory tracking control can ensure the performance matching and safety of the motor and the track vehicle during the operation. 4 EXPERIMENTAL VERIFICATION To verify the performance of the trajectory tracking control system in a real environment, in this section, a) d) Fig. 8. Spiral-line tracking result; a) trajectory comparison, b) velocity fluctuation, c) distance error, and d) angle error we designed a set of the proportional model test device. Set different predetermined trajectories and use visual recognition to obtain lateral vehicle deviation and angular deviation. The Kalman filter b) c) Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 337 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 is used for reducing signal interference, and the trajectory tracking predictive control algorithm is used to control and verify the performance of the control system. 4.1 Track Test Device The experiment devices are shown in Fig. 9, including the crawler, stepper motor, driver, camera, etc. The crawler's parameters are shown in Table 1. The stepper motor model is 42BYG34-401A, and the camera resolution is 1920*1080. Fig. 9. Track test device Table 1. Parameters of the crawler Name Value [mm] Center distance 220 Track shoe width 40 Total length 280 Drive wheel circumference 124 Track grounding length 140 Total height 90 Fig. 10. Predictive control system structure The predictive control system uses the camera to capture the target trajectory of the ground mark. After processing with LabVIEW, use MATLAB and combine the track vehicle kinematics model and Kalman filter to calculate the current lateral deviation and angular deviation. Use MATLAB to give the control amount. LabVIEW inputs the control amount to the controller NI cRIO through the serial port, and cRIO converts it into the corresponding electric signal input to the stepper motor, which drives the track forward. The structure of the control system is shown in Fig. 10. 4.2 Monocular Visual Deviation Calculation The experiment uses monocular visual to ranging. During the crawler's movement, the camera shoots the ground mark at the regular time. LabVIEW converts the image into a black and white binary image and fits the target trajectory line, as shown in Fig. 11. A a) b) K Fig. 11. Dimension relation; a) binary image, b) camera vision In the camera's vision, a horizontal line is preset corresponding to a virtual line on the ground, as shown by the green line (line A'B') and line AB in Fig. 11. Point K is the position of the crawler, point O is the position of the camera, and line m is the reference trajectory which is pre-paved on the ground by the paper tape. By measuring the angle between the straight line A'B' and fitted line m, and the distance d' between the vertical line and the line m on A'B', the lateral deviation l and angular deviation ( can be obtained with the principle of similar triangles. The calculation method is as follows: n n a =--ß 2 d = d' ■ AB (42) . d tana + f l =-— + e tana In Eq. (42), d' is the fitting pixel distance and d is the virtual deviation; AB is the distance of the line pixel; f is the distance between line AB and the centre of the track, and e is the lateral deviation of the camera installation position from the centre of the track. 4.3 Algorithm Based on Kalman Filter During the movement of the track vehicle, it is difficult to obtain accurate system state variables due 338 Zhou, L. - Wang, G. - Sun, K. - Li, X. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 to vibration errors, straight-line fitting errors, and the random noise of the camera. Using a Kalman filter, the optimal weighting matrix can be calculated by iteration. Then by weighting the measurement and the predicted state, the optimal state estimation can be obtained. Using the linearized kinematics model for predictive control, the state equation of the system can be written as: Xk = Ak Xk-1 ' y k = Cxk Bk uk-i (43) where yk is the output, and C = diag(0, 1, 1). Assume the system have input noise wk-1 and observation noise vk, then the system state equation is: ik = Akxk-i' y k = Cx k+v k Bk uk-i ■w. (44) The Kalman filter uses the recursive method to find the estimated value of the state quantity under the minimum mean square error. The recursive equation is: P 'k = AkPk-1 Ak + Qk-1 H k = P 'k C[ (Ck P 'k Ck + R k )-1 x k = A k x k-i + Bk Uk-i-Pk = (I - H k Ck )P 'k -H k (y k- CkA kx k-i) (45) where Qk-1 and Rk are the variance matrix of the input noise wk-1 and the observation noise vk, respectively; Hk is the gain matrix and essentially a weighting matrix; P'k and Pk are the covariance matrix of the uncorrected and corrected state variable error estimates respectively. 4.4 Experiment Analysis To verify the predictive control performance, the trajectory-tracking experiments are carried out under uniform linear motion condition with different target vehicle velocities and different initial poses. According to the previous algorithm, write the corresponding MATLAB and LabVIEW program. Fig. 12 shows the 30 s trajectory tracking control result of the track vehicle at the reference vehicle velocity of 0.03 m/s, the initial lateral error of -0.3 m and initial angular error of n / 3. Figs. 12a and b show the lateral deviation curve and angular deviation curve under this condition. Due to the vehicle vibration, ground interference, and camera error, the tracking error has some fluctuations. After the Kalman filter, a smooth error curve can be obtained. At the beginning of the vehicle motion, the initial pose error is large because the initial pose is inconsistent with the initial target pose. After 20 s, the system realizes the accurate trajectory tracking, and then the lateral deviation and angular deviation are basically zero. a) b) c) Fig. 12. Straight line tracking result I, a) lateral deviation, b) angular deviation, and c) velocity fluctuation Fig. 13 shows the trajectory tracking control result of the track vehicle at the reference vehicle velocity of 0.06 m/s, initial lateral error of -0.5 m and initial angle error of n / 4. It can be seen from the figure that the system overcomes the initial lateral angular deviation at about 15 s and realizes the accurate trajectory Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 339 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 tracking. After that, the deviation remains zero. It can be seen from Fig. 12 and Fig. 13 that the predictive controller based on monocular vision and the Kalman filter algorithm can realize accurate trajectory tracking and has good stability. a) b) Fig. 13. Straight line tracking result II; a) lateral deviation, b) angular deviation, and c) velocity fluctuation 5 CONCLUSION For the trajectory tracking control of track vehicles, this paper proposes a model predictive control algorithm based on state-space model. The algorithm uses the linear kinematics model of the track vehicle, and discretizes the state equation for predictive control of the vehicle. First, this paper establishes the kinematics model of the track vehicle, and combines the vehicle dynamics model and the dynamic model of the induction motor to establish the electromechanical coupling dynamics model of the track vehicle. Based on the linear kinematics model of the track vehicle, the kinematic and dynamic trajectory-tracking prediction controllers are designed. The controller performs rolling optimization at each sampling time according to the lateral deviation and angular deviation between the vehicle trajectory and the target trajectory, and adjusts the vehicle heading and velocity in real time. The kinematics and dynamics simulation experiments are designed using the proposed control algorithm, and are performed in a straight path and a spiral path. In the kinematic simulation, the crawler's velocity of both sides to track the target trajectory are adjusted. The dynamic simulation is tested for trajectory tracking by adjusting the voltage and frequency of the motor on both sides. The different lateral and angle deviations are set before the simulations start. The simulation results are as same as expected, showing that the algorithm can make the controlled vehicle overcome initial deviation and track the target trajectory in a short time, and maintain stable with no overshoot after tracking. To verify the control effect of the algorithm in real vehicles, the proportional model of the track vehicle is used for control tests. A predictive control trajectory tracking system for real vehicles is designed based on monocular vision and Kalman filter. The tests are carried out under different target velocities and different initial poses. The results show that the algorithm can realize the trajectory tracking of a real vehicle prototype, and the effects can meet the trajectory-tracking requirements. 6 ACKNOWLEDGEMENTS The research work financed with the National Natural Science Foundation of China (Grant No. 51775225) and key scientific and technological coal-based research projects in Shanxi (Grant No. MJ2014-02). 7 NOMENCLATURES vR, vL velocity on both sides, [m/s] R turning radius, [m] B track centre distance, [m] vc centroid speed, [m/s] mc centroid angular velocity, [rad/s] c) 340 Zhou, L. - Wang, G. - Sun, K. - Li, X. Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 q> angle between vc and the X axis, [-] b width of the track shoes, [m] l track grounding length, [m] m vehicle mass, [kg] Fqr, Fqi driving forces on both sides, [N] Frr, Frl running resistance on both sides, [N] Fsr, Fsl steering resistance on both sides, [N] Jc moment of inertia, [kgm2] d rotational mass factor, [-] Jm total moment of inertia, [kgm2] m motor shaft speed, [r/s] Tem motor electromagnetic torque, [Nm] Tl motor load torque, [Nm] fm rotational drag coefficient, [kgm2/s] n transmission system efficiency, [-] J moment of inertia of the motor shaft, [kgm2] r radius of the drive wheel, [m] i transmission ratio, [-] L inductance matrix of the motor, [-] R resistance matrix of the motor, [-] p0 number of pole pairs, [-] G rotational inductance matrix, [-] i current vector, [A] u voltage vector, [V] x(k) state vector, [-] u(k) control input, [-] y(k) control output, [-] A state matrix, [-] B control matrix, [-] C output matrix, [-] 8 REFERENCES [1] Marquardt, J.G., Alvarez, J., Von Ellenrieder, K.D. (2014). Characterization and system identification of an unmanned amphibious tracked vehicle. IEEE Journal of Oceanic Engineering, vol. 39, no. 4, p. 641-661, D0l:10.1109/ J0E.2013.2280074. [2] Stojkovic, V., Mikulic, D. (2002).The impact of a fixed kinematic turning radius of a tracked vehicle on the engine power required in a turn. Strojniški vestnik - Journal of Mechanical Engineering, vol. 48, no. 8, p. 459-466. [3] Beetz, A., Schwieger, V. (2013). Automatic lateral control of a model dozer. Journal of Applied Geodesy, vol. 7, no. 4, p. 257269, D0I:10.1515/jag-2013-0003. [4] Cook, J.T., Ray, L.E., Lever, J.H. (2016). Dynamics model for mobility optimization and control of off-road tractor convoys. Proceedings of the American Control Conference, p. 68756880, D0I:10.1109/ACC.2016.7526755. [5] Alvarez, J., Bertaska, I.R., Von Ellenrieder, K. (2013). Nonlinear control of an unmanned amphibious vehicle. ASME Dynamic Systems and Control Conference, p. V003T34A005, D0I:10.1115/DSCC2013-4039. [6] Luo, L. (2015). Adaptive cruise control design with consideration of humans' driving psychology. Proceedings of the 11th World Congress on Intelligent Control and Automation, p. 2973-2978, D0l:10.1109/WCICA.2014.7053202. [7] Yeu, T.K., Hong, S., Kim, H.W., Choi, J.S. (2005). Path tracking control of tracked vehicle on soft cohesive soil. Proceedings of the ISOPE Ocean Mining Symposium, p. 168-174. [8] Zou, T., Angeles, J., Hassani, F. (2018). Dynamic modeling and trajectory tracking control of unmanned tracked vehicles. Robotics and Autonomous Systems, vol. 110, p. 102-111, D0I:10.1016/j.robot.2018.09.008. [9] Huang, P., Zhang, Z., Luo, X., Zhang, J., Huang, P. (2018). Path tracking control of a differential-drive tracked robot based on look-ahead distance. IFAC-PapersOnLine, vol. 51, no. 17, p. 112-117, D0I:10.1016/j.ifacol.2018.08.072. [10] Chen, C.-Y., Li, T.-H.S., Yeh, Y.-C., Chang, C.-C. (2009). Design and implementation of an adaptive sliding-mode dynamic controller for wheeled mobile robots. Mechatronics, vol. 19, no. 2, p. 156-166, D0I:10.1016/j.mechatronics.2008.09.004. [11] Yandong, L., Zongyi, W., Ling, Z., Tao, L. (2011). Sliding mode control of mobile robots based on recurrent fuzzy-neural network. Journal of Jilin University (Engineering and Technology Edition), vol. 41, no. 6, p. 1731-1737, D0I:10.13229/j.cnki.jdxbgXb2011.06.022. (in Chinese) [12] Jeon, C.W., Kim, H.-J., Han, X.Z., Kim, J.-H. (2017). Fuzzy logic-based steering controller for an autonomous head - Feed combine harvester. ASABE Annual International Meeting, D0I:10.13031/aim.201700927. [13] Dai, Y., Zhu, X., Zhou, H., Mao, Z., Wu, W. (2018). Trajectory tracking control for seafloor tracked vehicle by adaptive neural-fuzzy inference system algorithm. International Journal of Computers, Communications and Control, vol. 13, no. 4, p. 465-476, D0I:10.15837/ijccc.2018.4.3267. [14] Endo, D., Okada, Y., Nagatani, K., Yoshida, K. (2007). Path following control for tracked vehicles based on slip-compensating odometry. IEEE International Conference on Intelligent Robots and Systems, p. 2871-2876, D0I:10.1109/ IR0S.2007.4399228. [15] Lenain, R., Thuilot, B., Cariou, C., Martinet, P. (2007). Adaptive and predictive path tracking control for off-road mobile robots. European Journal of Control, vol. 13, no. 4, p. 419-439, D0I:10.3166/ejc.13.419-439. [16] Bahadorian, M., Savkovic, B., Eaton, R., Hesketh, T. (2011). Toward a robust model predictive controller applied to mobile vehicle trajectory tracking control. IFAC Proceedings Volumes (IFAC-PapersOnline), vol. 44, no. 1, p. 13552-13557, D0I:10.3182/20110828-6-IT-1002.01786. [17] Shen, C., Shi, Y., Buckham, B. (2018). Trajectory tracking control of an autonomous underwater vehicle using Lyapunov-based model predictive control. IEEE Transactions on Industrial Electronics, vol. 65, no. 7, p. 5796-5805, D0I:10.1109/TIE.2017.2779442. [18] Falcone, P., Borrelli, F., Asgari, J., Tseng, H.E., Hrovat, D. (2007). Predictive active steering control for autonomous vehicle systems. IEEE Transactions on Control Systems Technology, vol. 15, no. 3, p. 566-580, D0I:10.1109/TCST.2007.894653. [19] Lee, S.H., Lee, Y.O., Son, Y., Chung, C.C. (2011). Proximate model predictive control strategy for autonomous vehicle lateral control. 11th International Conference on Control, Trajectory Tracking Study of Track Vehicles Based on Model Predictive Control 341 Strojniski vestnik - Journal of Mechanical Engineering 65(2019)6, 329-342 Automation and Systems, p. 590-595, D0l:10.1109/ acc.2012.6315465. [20] Kim, B.-A., Lee, S.-H., Lee, Y.O., Chung, C.C. (2012). Comparative study of approximate, proximate, and fast model predictive control with applications to autonomous vehicles. 12th International Conference on Control. Automation and Systems, p. 479-484. [21] Backman, J., Oksanen, T., Visala, A. (2010). Nonlinear model predictive trajectory control in tractor-trailer system for parallel guidance in agricultural field operations. IFAC Proceedings Volumes, vol. 43, no. 26, p. 133-138, D0I:10.3182/20101206-3-JP-3009.00023. [22] Kayacan, E., Ramon, H., Saeys, W. (2017). Robust trajectory tracking error model-based predictive control for unmanned ground vehicles. IEEE/ASME Transactions on Mechatronlcs, vol. 21, no. 2, p. 806-814, D0l:10.1109/TMECH.2015.2492984. [23] Guanyu, Z., Guoqiang, W., Huanyu, Z. (2017). Simulation analysis of electromechanical performance of crawler of mobile crushing station. Journal of the Brazilian Society of Mechanical Sciences and Engineering, vol. 39, no. 12, p. 4943-4954, D0I:10.1007/s40430-017-0907-5. [24] Kitano, M., Kuma. M. (1977). An analysis of horizontal plane motion of tracked vehicles. Journal of Terramechanics, vol. 14, no. 4, p. 211-225, D0I:10.1016/0022-4898(77)90035-0. [25] Wang, X., Taghia, J., Katupitiya, J. (2016). Robust model predictive control for path tracking of a tracked vehicle with a steerable trailer in the presence of slip. IFAC-PapersOnLine, vol. 49, no 16, p. 469-474, D0I:10.1016/j.ifacol.2016.10.085. 342 Zhou, L. - Wang, G. - Sun, K. - Li, X.