An Adaptive Weight Model Predictive Control Algorithm to Trajectory Tracking Control of UUV

Authors: danjie  zhu
hongtan  zhao
bing  sun
zinan  su
Venue: IEEE_org_ICIST_2024_Conference
Published: 2024-08-08 05:42:32

Abstract: Unmanned Underwater Vehicle (UUV) has been applied increasingly in marine work, and the trajectory tracking speed and accuracy directly affect work efficiency. To improve the convergence speed and accuracy of UUV tracking control, an adaptive weight model predictive control based on quantum particle swarm optimization (QPSO-AWMPC) is presented in this paper. The control weight is determined by the state tracking error. At the initial stage, where the tracking error is large, the weight control is lowered to achieve a higher speed. At the stable phase, where the tracking error is small, the weight control is raised to maintain robustness. A kinematics controller is designed to optimize state error and obtain the desired speed within constraints, while a dynamic controller is proposed to optimize speed and obtain the required thrust. Finally, the three-dimensional simulation verifies the effectiveness of the proposed method. Compared with the traditional MPC and Backstepping methods, the internal constraint of the UUV is satisfied while the convergence speed, accuracy, and robustness are improved.

Main paper: 
 i. introduction: 
 Unmanned Underwater Vehicle (UUV), due to the flexibility and autonomy, is used to explore the oceanic and scientific investigation. Trajectory tracking is an important aspect of the control research of UUV. Now many trajectory tracking methods have been proposed, such as PID control [1, 2] , backstepping control [3, 4] , neural network method [5] , sliding mode control [6] [7] [8] , etc. All of the above methods have been successfully applied in UUV trajectory tracking research.In addition to the above work, model predictive control has been widely used in UUV control owing to its own advantages of adding constraints. Model predictive control has the inherent advantage of dealing with the internal constraints of the machine directly [9] [10] [11] [12] . It has a strong adaptability to the nonlinear underwater environment, and can solve This project is supported by the National Natural Science Foundation of China (62033009) and the Creative Activity Plan for Science and Technology Commission of Shanghai (23550730300). the problems of speed jump and thrust saturation well in the tracking process [13] [14] [15] . Gan et al. [16] proposed the MPC method based on quantum particle swarm optimization (QPSO), which adopts the latest optimization algorithm, to achieve the faster tracing with constraints. In reference [17] , an observer was designed to improve the robustness of UUV by adding the influence of ocean currents. In reference [18] , a hybrid control strategy based on MPC is applied to 7000m-Human Occupied Vehicle and its feasibility is verified, and Zhu et al. proposed further model predictive cascade control for resolving actuator saturation in human-occupied vehicle trajectory tracking. Yao et al. [20] proposed an improved model predictive control and designed different weight for comparison according to different depths in the fixed-depth control. It verifies the feasibility of the method and gives the influence of different size weight. The above research solved the thrust saturation, but failed to improve the UUV speed control value. In addition, fixed weight leads to a decrease in UUV adaptive ability. Its tracking accuracy, convergence speed and robustness all need to be further improved.In this paper, inspired by adaptive weight control and constraint optimization of MPC, a hybrid control strategy QPSO-AWMPC which combines quantum particle swarm optimization (QPSO) and adaptive weight model predictive control (AWMPC) is proposed to deal with the tracking control problem of UUV. Then the SMC method was cascaded to realize UUV dynamic design. The overall strategy is comprised of two parts: the first part is the kinematics controller based on QPSO-AWMPC, which can get the desired speed according to the position information. The second part is the dynamics controller based on SMC, which can get the desired thrust according to the speed information.The rest of the paper is organized as follows. In Section II, the control principle of MPC, the optimization algorithm of QPSO, the simplified model and thruster arrangement of UUV are introduced. In Section III, we present the hybrid control strategy QPSO-AWMPC, and design the controller of kinematics and dynamics. In Section IV, the stability analysis of the controller is given. In Section V, to indicate the feasibility and effectiveness of the proposed method, simulation comparison and analysis are given. In Section VI, concluding remarks are made.For model predictive control, there are three key steps in the process, namely three basic principles, which are prediction model, rolling optimization and feedback correction, respectively corresponding to the prediction model, controller and corrector. The function of the prediction model is to predict the future output value. The controller identifies the characteristics of the system output. And the corrector is the feedback correction for the error.During the control process, there is always an expected reference trajectory used as the optimization target. Taking time k as the current time, the controller combines the current measured value and prediction model. To predict the output of the system in the future domain [k, k + N P ] (N P is the predicted time domain), a series of control sequences in the control time domain [k, k + N C ] (N C is the control time domain) is obtained by solving optimization problems satisfying objective functions and constraints.The first element of the control sequence is taken as the actual control quantity of the controlled object. When it comes to the next time k + 1, the above process repeats, completing an optimization problem with constraints in a rolling manner, thereby achieving continuous control of the controlled object.Quantum particle swarm optimization (QPSO) is a type of swarm intelligence optimization algorithm, which is developed from ant colony algorithm, particle swarm optimization algorithm, etc. The QPSO algorithm cancels the movement direction attribute of the particle. And the update of the particle position has nothing to do with the previous movement of the particle, increasing the randomness of the particle position. The biggest update is that the new quantity mbest is introduced to represent the average value of pbest. The objective function is optimized by QPSO algorithm to obtain the control sequence. The pseudocode is shown in Table I .The control increment V is initialized. Then calculating the fitness functions value of each group and finding out the optimal group. Depending on the corresponding formula, the above control increment is updated, the fitness function value is calculated, the optimal control increment is found and compared with the previous increment. The specific implementation process of QPSO method. In this way, when the cycle number reaches the maximum, the optimal control increment is achieved. Initialize velocity V i and position X i 3:Evaluate particle i and set P best i = X i 4: end for 5: Gbest = min(P best) 6: while not stop 7:for i = 1 to N 8:Update the velocity and position 9:Evaluate particle i 10:if fitness(X i ) < fitness(P best i ) 11:P best i = X i 12:if fitness(P best i ) < fitness(Gbest i ) 13:Gbest i = P best i 14:end for 15: end while 16: print Gbest C. UUV dynamics simplified model UUV moves in space with six degrees of freedom under water. Its coordinate system is divided into inertial frame and body-fixed frame, as shown in Fig. 1 . The motion in the body-fixed frame can be expressed by the linear motion along three axes and the rotation around three axes, and its nonlinear dynamic motion equation is as follows:EQUATION)where M is the inertia matrix, C(v) is the Coriolis term and the centrifugal term, D(v) is hydrodynamic loss term, g(η) is the resultant force term of gravity and buoyancy, τ is the input for propeller control, V = u v w p q r T .In practical applications, pitch and roll movements are rare, so this paper studied the relatively frequent motions, namely surge, sway, heave and yaw. The reference state is:EQUATIONThe real-time state is: The errors of x, y, z, ψ are respectively e x , e y , e z , e ψ . The state vector in the inertial frame is expressed as η =EQUATIONx y z ψ T . The speed vector in body-fixed frame isexpressed as v = u v w r T .The correlation is:EQUATIONThat is:EQUATIONwhere J(η) is the Jacobian matrix, which is generally used as the transformation matrix from the body-fixed frame to the inertial frame.To check the effectiveness of the proposed algorithm, we used the data of Falcon vehicle for simulation, so the thruster system of Falcon is analyzed. The structure and horizontal thruster layout of Falcon are shown in Fig. 2 .Falcon unmanned underwater vehicle contains 5 thrusters, 4 thrusters in the horizontal plane and 1 thruster in the vertical plane. It can carry out 4-dof movements: surge, sway, yaw and heave. a and b are width and length of Falcon, α is angle between the thruster and the X 0 axis of the body-fixed frame. The specific data are as follows: a = 0.6 m, b = 1 m, and α = 36 • .The hydrodynamic parameters of Falcon can refer to [16] . The simplified dynamic model used in the simulation is as follows:       (m + X u) u + X u u + X uu u|u| = τ X (m + Y v ) v + Y v v + Y vv v|v| = τ Y (m + Z ẇ) ẇ + Z w w + Z wv w|w| = τ Z (I z + N r ) ṙ + N r r + N rr r|r| = τ ψ (6)According to the propeller layout of Fig. 2 and the principle of dynamic, the resultant force and resultant moment generated by Falcon on each degree of freedom can be easily calculated: EQUATIONEQUATION)where τ is the normalized form of τ , and T is the normalizedEQUATIONThe objective function is designed as:J(k) = Np i=1 ∥η(k + i | t) -η ref (k + i | t)∥ 2 Qη + Nc-1 i=0 ∥∆V (k + i | t)∥ 2 Rr (11)The first term is the path tracking error penalty term, whose physical meaning is the ability to track the expected path. Q η is the state weight matrix. In this design, Q η has a fixed value. The second is the control input penalty term, which suppresses the excessively drastic control increment to ensure that there will be no big jump at the control input port, making the input performance more stable. R v is the control input weight matrix. In this design, R v takes variable values.In most studies, the weight matrix is fixed, like in references [11] , [16] . Considering the actual track tracking process, the controller has different requirements for different stages of UUV operation. There is a large error in the initial state of tracking, while the controller expects the UUV to approach the reference trajectory at a higher speed. The tracking error is small in the late stage, while the controller expects the UUV to operate at a more stable state and speed. To meet this demand, the MPC method based on adaptive weight is designed in this paper. The control weight matrix R v is designed to be variable, which makes UUV converge at a relatively higher speed and operate in a more stable state. UUVs specific constraint must be met in the process, and the MPC method handles this well.This paper introduces a variable parameter δ v . Weight R v is expressed as:EQUATIONI m×m is the m-dimensional identity matrix, and m is the number of controlled-quantity. The relationship between δ v and state error e η (t) is shown in Fig. 3 .Here, e η (t) = x -x d y -y d z -z d ψ -ψ d , e η = ∥e η (t)∥ , e η1 , e η 2 is the state error of different sizes, and e η1 < e η2 . When ∥e η (t)∥ ≥ e η2 , the value of δ v approaches the minimum value of 0 , so as to give a large control increment and improve the tracking speed. When ∥e η (t)∥ ≤ e η1 , the value of δ v approaches the maximum value of 1 , so as to suppress large fluctuations of control variables and improve stability. When e η1 < ∥e η (t)∥ < e η2 , there is a law of change. Where 0 and 1 are theoretical limits, the actual value is close to 0 and 1. The law of global change is expressed as follows:EQUATIONThe entire system design flow chart is shown in Fig. 4 , and it is subdivided into kinematics controller design as the outer loop and dynamics controller design as the inner loop.Here, η d is the reference state, e η is the state error, v c is the expected speed, η is the real-time state, v is the real-time speed. The kinematics controller is comprised of error model, system constraint and objective function. The state error in the inertial frame is optimized, while the kinematics controller is constructed to obtain the desired speed v c .1) Error model: The optimal object for kinematic tracking is the state variable error. The UUV tracks the reference trajectory on the status by narrowing the error. Reference state and the actual state are η d (t),η(t) respectively, So the state error is defined as:EQUATIONWhen t tends to infinity, the state error of UUV tends to 0, that is:EQUATIONWith the speed as the input of the system and the state as the output of the system, the real-time system can be expressed as follows:EQUATIONEach point needs to satisfy the above equation, then the reference trajectory can be expressed as:EQUATIONIn general, Taylor expansion method is employed to error analysis. Carrying out Taylor expansion of equation ( 16) at the reference point and ignoring higher-order terms:EQUATIONEquation ( 17) minus equation ( 16) is equal to:EQUATIONwhere:A(t) =     0 0 0 -u sin ψ -v sin ψ 0 0 0 u cos ψ -v cos ψ 0 0 0 0 0 0 0 0     B(t) =     cos ψ -sin ψ 0 0 sin ψ cos ψ 0 0 0 0 1 0 0 0 0 1    Then, the above equation can be expressed as:EQUATIONIn fact, the controller design is a discrete sampling, so the first-order difference quotient method is adopted to discretize the continuous equation:EQUATIONEQUATIONT is the sampling time, that is:A k,t =     1 0 0 (-u sin ψ -v sin ψ)T 0 1 0 (u cos ψ -v cos ψ)T 0 0 1 0 0 0 0 1     B k,t =     T cos ψ -T sin ψ 0 0 T sin ψ T cos ψ 0 0 0 0 T 0 0 0 0 T    Then, equation ( 20) can be expressed as:EQUATION2) Solution of control: We change equation ( 23) to the following form and get an augmented state-space model:EQUATIONThe augmented model can be written in the following matrix form:EQUATIONwhere,Ãk,t = A k,t B k,t 0m × n I m , Bk,t = B k,t I m , Gk,t = G k,t 0 , m = 4, n = 4.The predicted outputs sequence is in the finite prediction layer N P and the control input sequence is in the finite control layer N C :The output sequence is:γ = η(k + 1 | k) η(k + 2 | k) • • • η (k + N p | k) T (27)The control input sequence is:∆V = ∆v(k) ∆v(k + 1) • • • ∆v (k + N c -1) T (28)We combine equations of (25), (26), ( 27) and (28) to obtain the system prediction output:EQUATIONwhere Ψ t =   Gt,t Ãt,t • • • Gt,t ÃNp t,t   Θ t =          Gt,tEQUATIONwhereH t = [ Θ T t QΘ t + R 0 ], G t = [ 2E(t) T QΘ t 0 ]. The simulation constraints are set as follows. The control quantity constraint:EQUATIONThe control increment constraint:EQUATIONBased on lots of data, we set v max = 2 m/s. The terminal item of prediction output γ is as follows, which is set as 0.η (k + N p | k) = Gt,t ÃN P ξ(t, t) + Gt,t ÃN P -1 t,t Bt,t • • • Gt,t ÃN P -N C -1 t,tBt,t ∆V = 0 (33) In conclusion, the kinematic trajectory tracking problem can be transformed into the following optimization problem.min ∆V J(k) = ∆V (t)H t ∆V (t) T + G t ∆V (t) T s.t. Eq(31) -Eq(33) (34)The control sequence for a period of time in the future can be obtained by solving this equation with QPSO algorithm. Then, its optimal solution can be obtained and the first increment of ∆V * is applied as the control input:EQUATIONKinematics system output is the desired speed. In order to get thrust to reach the desired speed, a dynamic controller needs to be built. The design of the controller adopts the sliding mode control method.Due to the chattering caused by switching terms in traditional SMC, adaptive continuous switching terms are designed in this paper to avoid intermittent jumping.v and v c are the real-time speed and desired speed of UUV respectively. Then the speed tracking error is defined as:EQUATIONSliding mode control includes the design of sliding mode surface and control law. Firstly, the design of sliding mode surface is as follows:EQUATIONWe take the derivative of S and set it to 0:ṡ = ëc + 2Λ ėc + Λ 2 e c = ëc + 2Λ ( vc -v) + Λ 2 e c = 0 (38)The known kinetic equation is:EQUATIONWe put equation (40) into equation (38) to get:EQUATION)Because the UUV dynamic model is not completely known, the dynamic equation is equivalent to the estimated term and the unknown term, so:τ = τ + τ (42)where, τ = M q + Cq + Dq + g + τ w ; τ = M q + Ĉq + Dq + ĝ; M, C, D, g are estimate term of M , Ĉ, D, ĝ, M, C, D, g are unknown term of M , C, D, g, τ w is unknown disturbance. The control law is designed as follows:τ eq = M vc + ëc 2Λ + Λ 2 e c + Ĉq + Dq + ĝ (43)Because UUV runs slowly underwater, the unknown term can be considered to be very small and bounded. In order to ensure the stability of the sliding mode surface, the adaptive term is designed and added into the control law.τ ad = Γs + K + Ĉ 2Λ s (44)The complete control law is:EQUATIONThis experiment compared the improved QPSO-AWMPC method with the fixed-weighted QPSO-MPC and backstepping methods. The experiment was split into three-dimensional straight line and spiral curve simulation, respectively to verify the effect of speed jump or thrust saturation problems and convergence speed improvement.Related parameters: the sampling period was set as T s = 0.1 s, while sampling time is T = 200 s and T = 500 s. Backstepping parameters: Λ = 3, K = 60, Γ = 100. AWMPC parameters: population size popsize = 40, maximal iterations M AXIT ER = 50, predictive domain N p = 10, control domain N c = 10; dynamics related parameters: Λ = 3, K = 60, Γ = 100. Due to the insignificant changes of all parameters after a period of simulation, the intercept time of some simulation diagrams is less than the total simulation time.An important point is explained here in advance. If backstepping method does not take account of the actual constraints, the reference trajectory will be tracked quickly. In order to demonstrate the actual situation, we will limit the maximum thrust of the simulation.1) The control law of backstepping is:v c =     u c v c w c r c     =     k (V x cos ψ + V y sin ψ) + (u d cos e ψ -v d sin e ψ ) k (-V x sin ψ + V y cos ψ) + (u d sin e ψ + v d cos e ψ ) w d + k z V z r d + k ψ V ψ     (46)where k, k z , k ψ is the positive constant.2) The weight in method QPSO-MPC is fixed value 0.5. The tracking result is given in Fig. 5 . The blue dotted line represents the tracking trajectory generated by backstepping method. It can be seen that its track trajectory is divergent in finite space and cannot be converge, which is caused by the constraint of its thrust. The other two methods can track the reference trajectory stably. And the proposed QPSO-AWMPC tracking effect is better. Because backstepping method cannot track the reference trajectory, we will not talk about it in error and speed analysis. Fig. 6 shows the error variation in the tracking process. Taking the X direction as an example, we compare the tracking accuracy of the two methods with a fixed horizontal axis and find that the error of QPSO-AWMPC is always smaller than that of QPSO-MPC at the same time. Selecting 20s as an example to compare the error accuracy and the results are shown in Table II . We fixed the vertical axis to compare the taken time when the error was about 0, and found that QPSO-AWMPC method could converge to the vicinity of 0 faster. Taking the Y direction as an example, final convergence time of QPSO-AWMPC and QPSO-MPC is 48.6s and 104.2s respectively, and the convergence time is shown in Table III . This method saves a lot of tracking time and has a higher tracking accuracy.The speed changes can be observed in Fig. 7 . The black dot line marks the maximum speed that the QPSO-MPC method can achieve, and the blue solid line marks the maximum speed that the QPSO-AWMPC method can achieve. In large error phase, the speed of latter is always greater in four directions at the same time. In small error phase, such as 15-30s, the latter speed is lower than the former, which is caused by the latter larger control weight. Due to the large error of X and Y axis directions, the speed difference in these two directions is more obvious in the tracking initial stage: in the Y direction, the maximum speed of QPSO-AWMPC reaches about 0.9954 m/s, while the maximum speed of QPSO-MPC is about 0.5941 m/s. And the maximum speed of two methods satisfies the constraint. In the tracking stable phase, the two speeds are nearly equal. This speed change shows that QPSO-AWMPC converges faster in the early stage of the tracking process. Maximum speed value of UUV is shown in the Table IV . Fig. 8 shows the thrust change after normalization. After cutting off the over-limit thrust of backstepping method, the track diverges and the subsequent thrust changes greatly. The thrust of both QPSO-MPC and QPSO-AWMPC is within the constraint range. In the initial stage, the thrust of QPSO-AWMPC is larger than that of QPSO-MPC, while the thrust of tracking stable phase is almost equal. The thrust magnitude reflects the speed magnitude, so this result proves that QPSO-AWMPC tracking converges faster again. The maximum thrust value is illustrated in Fig. 9 .In conclusion, backstepping cannot track the reference trajectory under the condition of limited thrust. QPSO-MPC can track the reference trajectory well, but its convergence speed and tracking accuracy are far less than QPSO-AWMPC.By setting the initial state of UUV as (-10, 10, 0, 1), the reference trajectory is:η d (t) =        x d = 25 * sin(0.02 * t) y d = -25 * cos(0.02 * t) z d = 0.1 * t psid = 0.02 * t ,The simulation results are shown in Figs. 10 11 12 13 . Fig. 10 shows the tracking results of the three methods in the presence of thrust constraints. The backstepping method cannot effectively track the reference trajectory under the thrust constraint. However, both QPSO-AWMPC and QPSO-MPC can track the reference trajectory. QPSO-AWMPC has a better tracking result than QPSO-MPC.Fig. 11 shows the normalized thrust change. In order to respond to the actual situation, the thrust generated by backstepping is limited to [-1, 1]. So thrust constraint makes it can't achieve trajectory tracking process. Both QPSO-AWMPC and QPSO-MPC are restricted within the constraint, and the former has bigger thrust.From the error changes in the four directions in Fig. 12 , it can be seen that the error reduction speed of QPSO-AWMPC method is greater and the tracking accuracy is higher. In four directions, at the same time, the error brought by QPSO-AWMPC is always smaller than that of QPSO-MPC, and the former fluctuation is slight, so the final error is closer to 0. The error accuracy in 20s is shown in Table V . The convergence time can also be obtained from the error variation and is shown in Table VI . Fig. 13 shows the tracking real-time speed, and the initial real-time speed which represents the tracking convergence speed. As can be seen from the figure, speed of QPSO-AWMPC in all four directions is greater than that of QPSO-MPC method, while the latter speed is basically same.To verify tracking stability, we added disturbance at the 320s to simulate thruster failure. Observing trajectory trends and the result was shown in Fig. 14 . In the process of UUV tracking, we alter its speed. At this time, the UUV cannot reach the expected speed, so the tracking trajectory will deviate from the reference trajectory. From the degree of trajectory deviation, it can be seen that the tracking trajectory of QPSO-AWMPC is closer to the reference trajectory, with smaller relative error and higher accuracy. When the error reaches the maximum point, the trajectory descent gradient of QPSO-AWMPC is grater, that is to say, its convergence speed is faster than that of QPSO-MPC. In conclusion, the improved method had better robustness, accuracy and speed.Through simulation experiments of straight line and spiral curve tracking, on the basis of solving the problem of thrust saturation, the improved method can track the reference trajectory quickly with higher accuracy. The convergence time is about 50-60s less than QPSO-MPC method. Simulation experiment of robustness analysis shows that the robustness of UUV tracking process in the later stage is better, and the recovery speed is faster. The simulation results verify the effectiveness of the QPSO-AWMPC method.In this paper, an improved MPC method based on QPSO is designed to improve the convergence speed and accuracy of UUV trajectory tracking. In this method, the control weight is a function of state error. In the initial stage, if the error is large, the weight is small and the tracking speed is greater. In the later stage, if the error is small, the weight is large and the operation robustness is greater. Finally, the effectiveness of this method is verified by MATLAB simulation. This paper studied the convergence speed, accuracy and robustness deeply, and the fault tolerant control of tracking will be further discussed in the future work. 
 
 Appendix:  



Reviews: review: title: title: An Adaptive Weight Model Predictive Control Algorithm to Trajectory Tracking Control of UUV
main_review: main_review: In this paper, an adaptive weight model predictive control based on quantum particle swarm optimization (QPSO-AWMPC) is presented.
a There are some grammatical mistakes and typos. Please examine the full text further and revise them.
b The references should be updated. Some closely related and new references should be added to show to further explain the novelty and innovation of the work.
title: title: In this paper, an adaptive weight model predictive control based on quantum particle swarm optimization (QPSO-AWMPC) is presented.
main_review: main_review: a There are some grammatical mistakes and typos. Please examine the full text further and revise them.
b The references should be updated. Some closely related and new references should be added to show to further explain the novelty and innovation of the work.
c What are the significant differences between this study and previous studies? The author needs more explicit emphasis.
title: title: The work presented in this paper is significant as it addresses key challenges in UUV trajectory tracking, such as handling non-linearities, internal constraints, and improving convergence speed and accuracy.
main_review: main_review: The work presented in this paper is significant as it addresses key challenges in UUV trajectory tracking, such as handling non-linearities, internal constraints, and improving convergence speed and accuracy. The integration of QPSO with an adaptive weight mechanism provides a novel solution that adapts to different stages of UUV operation, making it more efficient and reliable in real-world applications. The simulation results further confirm the superiority of the proposed method over traditional approaches. However, future work could focus on experimental validation and exploring other optimization techniques to further enhance the control strategy. Below is a list of comments that should be taken into account further when revising the paper.
1.	Improper use of articles or long sentence structures with separators might make it difficult to follow the paper. For example, the use of ‘the’ in the paper and the presentation of subordinate clauses. Please check the full text again and modify the grammar problems.
2.	Please highlight the contributions of the paper.
3.	Please add the necessary comments for Figures.
4.	Meanwhile, please elaborate on the future plans.